Horizon level calibration should be applied to attitude estimator outputs

[priseborough]

Describe the bug

When the horizon level calibration step performed by the do_level_calibration() function runs, the resulting offsets SENS_BOARD_X_OFF and SENS_BOARD_Y_OFF are applied to the IMU. This has the effect of rotating the EKF's body frame of reference which moves with the IMU and results in other body frame sensors such as the magnetometer then being misaligned with the IMU and producing larger innovation levels.

The EKF does all its internal calculations in the IMU frame of reference so we either need to rotate other autopilot fixed sensor data into the new body frame of reference created by rotating the IMU data

OR

Rotate the EKF attitude solution before is is published in the vehicle_attitude.msg. This is the preferred solution.

Another issue is that the do_level_calibration() uses the attitude estimator (EKF) which creates an unnecessary dependency on gyro bias errors.

[dagar]

Let's go with the preferred solution. I'd like to figure out how to optionally do it in a multi-EKF world (stored per IMU).

Another issue is that the do_level_calibration() uses the attitude estimator (EKF) which creates an unnecessary dependency on gyro bias errors.

Are you suggesting to only use the gravity vector?

EDIT: Can you elaborate on why it's the preferred solution? Numerically preferable to not unnecessarily manipulate sensor data before sent to the EKF or perceived difficultly in having to adjust all data sources?

[dagar]

Rotate the EKF attitude solution before is is published in the vehicle_attitude.msg. This is the preferred solution.

A minor complication that comes to mind is keeping this sorted across the various messages. For example vehicle_imu (currently adjusted by SENSBOARD{X,Y}_OFF) is streamed by Mavlink as HIGHRES_IMU.

Maybe a new sensor_imu would be more appropriate.

[mhkabir]

For example vehicle_imu (currently adjusted by SENSBOARD{X,Y}_OFF) is streamed by Mavlink as HIGHRES_IMU.

Does it matter if the "raw" data isn't adjusted by that offset anymore? I know that the message description says "in NED body frame", but the actual real world usage of the message (what I've seen, anyway) for VIO, offboard optical flow, etc. treat it as raw IMU readings in the IMU frame. Also what even is a "NED" body frame. Probably "FRD" is more appropriate.

A very real thing which we encountered was our camera-IMU extrinsic calibrations would get changed due to level horizon. People did a level horizon through QGC, expecting better leveling in manual/attitude controlled modes, but it actually ended up changing the raw IMU data and messing with things downstream.

I agree with Paul's preferred solution of applying it to the attitude estimate such that it's only applied to the signal path going to the controllers, and not the estimator.

[dagar]

Does it matter if the "raw" data isn't adjusted by that offset anymore? I know that the message description says "in NED body frame", but the actual real world usage of the message (what I've seen, anyway) for VIO, offboard optical flow, etc. treat it as raw IMU readings in the IMU frame. Also what even is a "NED" body frame. Probably "FRD" is more appropriate.

We can implement whatever's needed and optimal, let's just avoid stretching any existing definitions for convenience.

The vast majority of SENS_BOARD_X/Y/Z_OFF usage seems to be fine level sensor adjustment (usually a couple degrees), but there are also people using this to support custom board mounting (eg 18 degree pitch).

[bresch]

The vast majority of SENS_BOARD_X/Y/Z_OFF usage seems to be fine level sensor adjustment (usually a couple degrees), but there are also people using this to support custom board mounting (eg 18 degree pitch).

That should be done using a custom board rotation... or maybe should we then keep SENS_BOARD_X/Y/Z_OFF for the autopilot orientation fine tuning and have two more parameters for the horizon roll/pitch offsets?

[dagar]

That should be done using a custom board rotation... or maybe should we then keep SENS_BOARD_X/Y/Z_OFF for the autopilot orientation fine tuning and have two more parameters for the horizon roll/pitch offsets?

A user accessible mechanism to set custom orientations for the board and all sensors (per instance) would be nice.

I don't think I have an opinion about SENS_BOARD_X/Y/Z_OFF vs new parameters yet. At the moment I'm more concerned about how we'll untangle the messages without creating more of a confusing mess.