priseborough
commented
2 years ago I don't think the sign for that correction is incorrect. The velocity of the IMU relative to the body frame origin is omega cross offset, where omega is the angular velocity vector in body frame and offset is the position of the IMU relative to the body frame origin in body frame. This calculation wants the velocity of the body frame origin wrt the IMU, hence the sign change. However this assumes the cross product implementation overloaded onto the % operator is correct.
A quadplane with IMU placed 0.5m in front of the CoG and GPS placed 1.2m to the left of the CoG had high EKF velocity noise leading to an oscillation on the attitude demand coming via the VTOL velocity controller in QLOITER mode. The noise was substantially reduced by setting INS_POS1_X to -0.5 instead of the correct +0.5. This was reflected in noticible roll oscillation when the INS pos was set correctly, and the oscillation stopped when the INS pos was set incorrectly to -0.5. Running replay with the sign changed at this line: https://github.com/ArduPilot/ardupilot/blob/Plane-4.2/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp#L894 shows this:
which seems to support the calculation there being incorrect.
I also think we should consider using filtered gyro data for the velocity correction. This velocity correction code is adding velocity noise proportional to the product of the gyro noise and the IMU distance from the CoG. A low pass filter on the gyro before we calculate this velocity correction would add a bit of lag, but would likely create a velocity estimate that is much better for the VTOL position controllers.
replay log here:
http://uav.tridgell.net/tmp/2022-05-06%209%20QLOITER%20ek3%20vs%20ek2.BIN