IMU POS compensation - Githubissues

Bug report

Issue details

According to the description about the IMU model in the book "STATE ESTIMATION FOR ROBOTICS by Timothy D.Barfoot".

The IMU model should be described as below:

render

do first derivative about this equation, we get

But in the function void NavEKF2_core::calcoutputStates():


// If the IMU accelerometer is offset from the body frame origin, then calculate corrections
    // that can be added to the EKF velocity and position outputs so that they represent the velocity
    // and position of the body frame origin.
    // Note the * operator has been overloaded to operate as a dot product
    if (!accelPosOffset.is_zero()) {
        // calculate the average angular rate across the last IMU update
        // note delAngDT is prevented from being zero in readIMUData()
        Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);

        // Calculate the velocity of the body frame origin relative to the IMU in body frame
        // and rotate into earth frame. Note % operator has been overloaded to perform a cross product
        Vector3f velBodyRelIMU = angRate % (- accelPosOffset);
        velOffsetNED = Tbn_temp * velBodyRelIMU;

        // calculate the earth frame position of the body frame origin relative to the IMU
        posOffsetNED = Tbn_temp * (- accelPosOffset);
    } else {
        velOffsetNED.zero();
        posOffsetNED.zero();
    }

and

// return the NED velocity of the body frame origin in m/s
//
void NavEKF2_core::getVelNED(Vector3f &vel) const
{
    // correct for the IMU position offset (EKF calculations are at the IMU)
    vel = outputDataNew.velocity + velOffsetNED;
}

From the equation, I think the function getVelNED is wrong, should be vel = outputDataNew.velocity - velOffsetNED; , because the function should return the vehicle velocity in NED not the IMU velocity in NED.

Version What version was the issue encountered with copter-3.6.7

Platform [ ] All [ ] AntennaTracker [x] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type What type of airframe (flying wing, glider, hex, Y6, octa etc)

Hardware type What autopilot hardware was used? (Pixhawk, Cube, Pixracer, Navio2, etc)

Logs Please provide a link to any relevant logs that show the issue

ArduPilot / ardupilot

IMU POS compensation #10936

Bug report