EKF does not account for IMU->CoG offset when returning NED acceleration

[CarlOlsson]

From ekf2_main in PX4 firmware
https://github.com/PX4/Firmware/blob/38da0f95aad2068645213d783e49663ab4105dbf/src/modules/ekf2/ekf2_main.cpp#L1296-L1301

// Acceleration of body origin in local NED frame
float vel_deriv[3];
_ekf.get_vel_deriv_ned(vel_deriv);
lpos.ax = vel_deriv[0];
lpos.ay = vel_deriv[1];
lpos.az = vel_deriv[2];

But this is not true. The function get_vel_deriv_ned returns the accelerations in the IMU frame not the body frame. (It does not account for the distance between IMU and CoG or compensates for the centripetal force)

[CarlOlsson]

I performed a test where a Pixracer was mounted on a stick 33 cm away from CoG and rotated around CoG back and forth. The acceleration in NE direction should hence be zero.

Orange is the estimated acceleration in North direction. Red is the derivative of the (filtered) estimated velocity in the north direction (which is compensated for IMU offset)

[dagar]

Just below the _vel_deriv_ned update in Ekf::calculateOutputStates() you can see the IMU offset (_params.imu_pos_body) used with the IMU delta angle.

https://github.com/PX4/ecl/blob/e1751188fd15b799cbfae86bd7373bb91206069b/EKF/ekf.cpp#L512-L519

Is the IMU position offset intentionally not used with IMU delta velocity or is it an omission?

https://github.com/PX4/ecl/blob/e1751188fd15b799cbfae86bd7373bb91206069b/EKF/ekf.cpp#L480-L491

[CarlOlsson]

I am not sure what the background is regarding this choice.
I did some more tests using the same data as above.
Blue is the raw acceleration measurements in the direction of the rotation and green is the measurements compensated for the IMU offset.

The compensation was done by low pass filtering the acceleration measurement and subtracting the position offset times the angular acceleration (obtained by taking the derivative of the filtered gyro signal).

[priseborough]

The acceleration returned is at the sensor frame. The only way to return acceleration at the body origin would be to use angular accelerations which is problematic due to the issues associated with differentiating rate gyro noise. This was tried in the past using the downsampled data available to the EKF but the resulting data product was noisy when engine vibration was present. It may be possible if we do the differentiation and filtering in the sensor driver on the high rate data before downsampling. This rate acceleration would then be need to be added to the sensor topic used by the EKF.

[dagar]

The only way to return acceleration at the body origin would be to use angular accelerations which is problematic due to the issues associated with differentiating rate gyro noise. This was tried in the past using the downsampled data available to the EKF but the resulting data product was noisy when engine vibration was present. It may be possible if we do the differentiation and filtering in the sensor driver on the high rate data before downsampling.

If anyone is interested in pursuing this we're now in a much better position to give it a try with the newer IMU drivers and sensor pipeline.

[priseborough]

Could be a good time to add this before the control loop designers start adding acceleration feedback. This feature will impact on tuning of acceleration feedback loops. Sometimes not correcting for the c.g. offset of the IMU can be an advantage, eg having an IMU in front of the c.g. that gives some phase lead for a tail controlled rocket.