Rover-4.1.0-beta6 released for beta testing!

I might ask @tridge and/or @priseborough to chime in here but I think it must be best to get the stable position during pivots and if this leads to oscillations in navigation then it should be possible to re-tune either NAVL1 param sor the turn rate controller and restore performance to what it was (or better).

So the key point I think is whether the GPS_POSn_X/Y/Z positions are relative to the autopilot’s IMU or relative to the middle of the rear-axle (aka COG in the docs). The docs say this:

In practice the distance to the sensor can be measured from the center of the autopilot unless the autopilot itself is placed a significant distance from the vehicle’s center of gravity in which case the IMU position offsets can be specified and then the other sensor’s position offsets can be specified from the vehicle’s center of gravity.

If we have an issue in the code I think it could be here in NavEKF3_core::CorrectGPSForAntennaOffset and this code does seem to use both the GPS offset and the IMU offset. TBH I would have thought that we shouldn’t have the IMU offset in the calc but I think I better let Tridge and Paul weigh in.

Txs for the report.

2 Likes

Randy, you and I interpret that line of code the same way - it makes perfect sense to explain the behavior I’m seeing.

const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gps_data.sensor_idx).toftype() - accelPosOffset;

Assuming the first term’s X component is my “cheater” offset of 0.2, and accelPosOffset.X = 0.27, then:

0.2 - 0.27 = -0.07 …or… the exact physical position of my GPS on the mower.

I’ve been meaning to set up a dev environment for a while now, and this is the perfect case for me to give it a shot. Thank you!

Also looking forward to further replies by the EKF masters - I definitely don’t want to induce unwanted behavior or error here.

As for tuning for perfect pivot positioning vs proper accelerometer offset, I agree with your assertion, in theory, that one should be able to tune the oscillation out, but I tried for a LONG time to do just that, and I settled on accepting the imperfect pivot turns as the best tune for my use case.

2 Likes

@rmackay, @tridge, @priseborough

I set a build environment up today and made that singular change to which Randy alluded. It completely fixed the strange behavior with respect to frame position offsets. Unless it creates unwanted side effects, line 330 of NavEKF3_core::CorrectGPSForAntennaOffset should read:

const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gps_data.sensor_idx).toftype();

The resulting tune (with the frame position offsets set per their measured/correct positions) made this excellent auto mission path:

Unrelated to this issue:
I first built the latest 4.2.0-dev master with ONLY this line 330 change incorporated, and I had a dangerous outcome. When I powered the mower, all of the relays were triggered opposite their previously default positions, simultaneously engaging the starter solenoid, PTO clutch, and exterior lights. I didn’t investigate too much because Randy mentioned it’s likely better to continue testing the beta releases for now, but I didn’t notice any significant parameter changes that would explain the behavior. A switch to the Rover-4.1 branch had the relays all behaving as expected again.

2 Likes

@Yuri_Rage,

Thanks for testing this, I’ll raise a PR shortly with the change and we can see what some of the other devs think (in particular Paul and Tridge).

We’ve recently changed some things to do with relays including removing the BRD_PWM_COUNT parameter. Perhaps something has gone wrong with that. I’ll ping @tridge.

2 Likes

Thanks, Randy, that was some quick work on your part to discover the likely culprit in the EKF3 code.

As for the 4.2.0 relay changes - it may not be that things have gone wrong so much as there may be some new defaults that are opposite the settings I presently have. Probably worth a word of caution if the parameters/settings are going to change significantly.

@Yuri_Rage,

Here’s the PR where some of the discussion will likely happen re the offsets.

I chatted with Tridge about the relay issue and we think, as you suspect, that it is the change in the defaults. In 4.1 we default the top four (?) servo output channels to be GPIOs but in 4.2 we removed BRD_PWM_COUNT and somewhat accidentally changed the default so all servo outputs are now PWM. We think if you set SERVOx_FUNCTION = -1 (GPIO) for the relevant channels then you’ll be back to the old behaviour. I’m not sure if you’ve got the appetite to try that or not…

We are thinking of ways we can make this transition safer and I suspect we will end up doing a parameter conversion and/or changing defaults to that these SERVOx_FUNCTION values are set automatically as part of the upgrade.

1 Like

Perfectly happy to lend a hand if that code is ready for testing. I can power the Cube via USB to make parameter changes without triggering relays. Just caught me completely off guard when I used my usual power-on process and all of a sudden things were spinning and flashing!

1 Like

@rmackay9, I read the PR, in which you state that either the code should be changed or the documentation changed. @Yuri_Rage needs to weigh in here in case I have it wrong, but I thought that BEFORE Yuri modified the code, he set the position parameters in a way that assumed that the IMU offset was included in the calculations and he got great pivot turns, but then his normal tracking had bad oscillations, implying that there are other places in the code that do not include the offset. (See the text beginning between the screenshots in post 24 above: Rover-4.1.0-beta6 released for beta testing! - ArduRover / Rover 4.1 - ArduPilot Discourse). If I am right, then a code fix would be required, not just a doc change.

I may be completely wrong. Yuri?

2 Likes

I strongly believe that the code change is necessary and appropriate.

First, to tackle the “easy way out” of changing the documentation alone: the way the EKF3 posOffsetBody is calculated, the documentation would become quite confusing in a hurry. While you can simply measure the IMU position with respect to the vehicle centroid and enter those values into the INS_POS* parameters directly as measured, you cannot for the GPS antennas. Nor can you simply measure the distance between the IMU and antennas - the subtraction won’t work. To properly set the GPS antenna position offset with the code as written, you need to make reference measurements to the vehicle centroid and then subtract those values from the IMU offsets to achieve what I believe to be the correct GPS_POS* parameters. I think I even confused myself just trying to write that.

As such, I agree with Randy’s comment in the PR discussion - it makes no sense to re-accomplish all sensor position offsets if the flight controller is re-mounted in another position on the vehicle. The reference point should always be the vehicle centroid, and the firmware should account for all offsets relative to that.

I think body position offset error becomes particularly evident during rover pivot turns, where the degrees of freedom are limited, the EKF must calculate position faster than a sensor (GPS) can bias it toward reality, and the vehicle has no ability to translate laterally (on the Y axis) in space, so any error results in undesirable course “corrections” upon exiting even the most physically perfect of pivot turns.

After reading some of the code, it’s still unclear to me why I had to fight so hard to tune the oscillation out of the nav controller with the GPS_POS* values referenced to the IMU, since the result should’ve been proper relative GPS position. I suspect that somewhere else in the control/performance loop(s), the GPS offset may be referenced to the centroid rather than to the IMU. Observed evidence after making the change to the code today strongly suggests that it results in desired behavior and also matches existing documentation.

1 Like

Flight vehicles ‘pivot’ about the centre of mass, ground vehicle with steered wheels rotate about the non steered axle. If the all the *_POS_X,Y,X params are set up relative to the non-steered axle, then a yaw rate won’t produce a cross-track velocity. This should make the L1 trajectory controller easier to tune. The c.g. position is largely irrelevant for this application.

2 Likes

It seem that the GS would just not discover the udp client/server, I’ll try to post the settings once I get the basic tuning done in 4.0.1 this week!

@Yuri_Rage are you able to provide a log with LOG_DISARMED=1 and LOG_REPLAY=1 with the IMU position, GPS position etc all set relative the centre of rotation? If you drive in RH and LH circles then I can determine by looking at the innovations and other data where the inconsistency is occurring.

1 Like

Code inspection shows that the WGS-84 output from the EKF is not being corrected for the IMU to body frame origin offset whereas local NED frame outputs were. This will be fixed by https://github.com/ArduPilot/ardupilot/pull/18264#pullrequestreview-725113538

3 Likes

To add some more detail to what Paul says, it turns out the issue was that we weren’t re-applying (un-applying?) the IMU offsets in the AHRS/EKF get_position() method.

So my suggestion was that we should move the GPS data to the middle of the vehicle and fuse it there. It seems the way the EKF is written though, we should first move the GPS to where the IMU is and fuse it there but then right at the end, once the EKF has created its position estimate, we move that whole estimate to the middle of the vehicle. So the IMU offsets are used twice… move all the sensor data to the IMU, then move the final produce to the middle of the vehicle.

Tricky stuff but super happy that we’ve gotten to the bottom of this.

3 Likes

Thank you all for taking a look!

@priseborough - would you still like those logs? The request appears overcome by events.

I should’ve chosen my words more wisely (above), and I see where I implied that center of mass might be important on a rover. My intent was center of Z-axis rotation, or the non-steered axle as you say (which could possibly confuse some folks when discussing skid steering).

I had a feeling the code fix might be a little more complex than the one I tested for myself, and I truly appreciate the more in-depth look.

I’ll build 4.1.0 today with the proper AP_NavEKF3_Outputs fixes in place of the AP_NavEKF3_PosVelFusion band-aid that Randy and I discussed and likely move the entire project to the 4.2.0-dev branch soon.

1 Like

@Yuri_Rage no log required for the time being given the fix.

1 Like

I’m also planning on releasing -beta7 some time this week and this will include the fix.

4 Likes

I can wait that long :smiley:

I thought perhaps it would be a 4.2 feature, since the PR changed a file that includes other 4.2 specific changes. I tried a 4.1.0-beta6 branch build with the entire file from the PR, and it broke due to compass function call changes, so I had to copy and paste the 3 changed lines into the 4.1 file. Glad it’s being incorporated sooner!

3 Likes

I was playing a bit with 4.1.0 beta6 and a u-blox F9P. The result is a tiny infinite WP-loop on the balcony, from which I was surprised that this is really working.
But see yourself: https://www.youtube.com/watch?v=Z0euGkhf5sk

3 Likes

@RainFly,

Re the tiny mission on the balcony, that’s quite impressive accuracy …