I also try to understand the EKF algorithm. In AP_NavEKF.cpp, in line 820 I found
prevDelAng = correctedDelAng; then in line 824
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
but from line 820 we know that prevDelAng % correctedDelAng = 0 so why is it here?
And with RecallStates() it seems that it deals with GPS latency compensation. However in this function we can find that
uint32_t bestTimeDelta = 200; then there is a check that
if (bestTimeDelta < 200) {latency compensation…}
else // otherwise output current state
{statesForFusion = state;}
that means the fusion is for the current state, no compensation at all.
My question is that in EKF algorithm, is there any kind of latency compensation or this only reserves for the future.
Does anyone know the answer?
The EKF design was my responsibility, so I will have a go at answering your questions.
“In AP_NavEKF.cpp, in line 820 I found
prevDelAng = correctedDelAng; then in line 824
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
but from line 820 we know that prevDelAng % correctedDelAng = 0 so why is it here”
That was a bug that prevented coning corrections from being applied. It is fixed in master, but I don’t know if the fix has made its way into 3.2. It has very little effect. Unfortunately the rates are filtered using a IIR filter before being used by the EKF, which means the coning errors are larger than the small amount compensated for by line 824.
“And with RecallStates() it seems that it deals with GPS latency compensation. However in this function we can find that
uint32_t bestTimeDelta = 200; then there is a check that
if (bestTimeDelta < 200) {latency compensation…}
else // otherwise output current state
{statesForFusion = state;}
that means the fusion is for the current state, no compensation at all.
My question is that in EKF algorithm, is there any kind of latency compensation or this only reserves for the future.”
This function tries to recall the stored state vector from the closest time to the measurement time stamp, however if it cannot get within 200msec of the measurement time stamp, it uses the current state. This is done for all measurements, so yes latency compensation (relative to inertial data) is currently implemented.
Paul, thank you so much for the reply. I used the old version of code and did not notice the fix . Now I’m better understanding the EKF topic. But if you don’t mind to write down the algorithm like in OpenPilot, it would be very much of help for many people who’d like to understand Ardupilot.
Yes, I have seen that. But here I mean a text description of code. As you mentioned in the post, instead of understanding Ardupilot code, people need to understand GenerateEquations22states.m, another kind of code which can’t be said that it is simpler.
From the number of viewers I see that many peoples are interested in EKF topic, so I try to describe the algorithm as I understand. Although this decription still lacks the information of measurement and process noise, and of covariance’s initialization due to my limitted understanding I hope that it still can help people in understanding Ardupilot and in the near future someone who better knows about covariance and noise will write down his knowledge for others.
I’ve come to the conclusion that there is no need for attachment as the text description of EKF algorithm is not so long so I can include all the text here.
The Kalman filter can be written as a single equation, however it is most often conceptualized as two distinct phases: “Predict” and “Update”. The predict phase uses the state estimate from the previous timestep to produce an estimate of the state at the current timestep. This predicted state estimate is also known as the priori state estimate because, although it is an estimate of the state at the current timestep, it does not include observation information from the current timestep. In the update phase, the current priori prediction is combined with current observation information to refine the state estimate. This improved estimate is termed the a posteriori state estimate.
Typically, the two phases alternate, with the prediction advancing the state until the next scheduled observation, and the update incorporating the observation. However, this is not necessary; if an observation is unavailable for some reason, the update may be skipped and multiple prediction steps performed. Likewise, if multiple independent observations are available at the same time, multiple update steps may be performed (typically with different observation matrices Hk)
It is known that GPS data are lagged after processing. When received that means the position in the past some time before. In the simple way this data can be fused as the current observation without any compensation for the latency. For slow moving objects this way may be all right, but for fast moving objects this may be problematic because the distance between real position and the received GPS data may reach many meters. The idea of compensation for GPS latency in the posteriori state estimate could be as follow: The update is based on the innovation (the difference between the observations and the estimations). As the measurement is delayed so at the moment of the fusion the observation is of the older moment, so from that moment on we need to rely on the estimation. Since the estimations are the same for 2 processes, we can assume that in the short time (<200ms in Ardupilot EKF) the estimation error is small enough and the current innovation = (the old observation + the new estimation) - (the old estimation + the new estimation) = the old observation - the old estimation = the old innovation.
Currently, in ardupilot EKF algorithm, prediction phase estimates the quaternion, velocity and position states using IMU measurements. Covariance prediction is performed if the total delta angle has exceeded the limit or the time limit will be exceeded at the next IMU update. When GPS data is received, it is fused to the predictions. The same GPS data is fused for many IMU steps for smoothness, GPS data are fused in the interval of velPosFuseStepRatio IMU steps that means one update step comes after velPosFuseStepRatio steps of prediction. Based on covariances, Kalman gains are calculated, then data fusion is performed and again covariance is corrected for the next covariance prediction.
To know the old estimations, states should be saved in buffer. The StoreStates() saves up to 50 states at 10 ms each, so it can save states in 500 ms interval. RecallStates() then calculate the state in the buffer which is closest to the time of GPS data and if closer than 200ms that are statesAtVelTime, statesAtPosTime, statesAtHgtTime. The innovations are then calculated in the past at those times, then we can fuse the observation to the current estimation with the innovation calculated. The fusions are repeated after velPosFuseStepRatio IMU steps (dtVelPos time) until the new GPS data is received or after a long time that makes the GPS data obsolete and we need to reset position and velocity and reset stored state history.
The fusions of magnetormeter and airspeed data are similar, with the fusion of magnetometer data is performed in 3 consecutive IMU steps to spread computational load.
@priseborough
Hello Paul,
After a long time of code reading, in the end I could understand the EKF algorithm coding.
I can say that it is excellent great amount of work. Before that I couldn’t think that magnetometer measurement can update the position.
However I think I’ve found a mistake in the coding. When correcting the states in line 2533 of AP_NavEKF.cpp with the scaling
magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * float(magUpdateCountMax) / minorFramesToGo);
imo the scaling factor here should be (magUpdateCountMaxInv * minorFramesToGo) so this line should be
magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * minorFramesToGo);
What’s your opinion?
After careful thinking I’ve come to the conclusion that it isn’t a mistake at all because the correction is taken place in every IMU steps so it’ll build up in time between 2 consecutive magnetometer readings
hello，I have been learning the ardupilot code recently , can I ask you a question about EKF algorithm? In ardupilot\libraries\AP_NavEKF\Models\QuaternionMathExample I found it use only one IMU as observation , my question is what if I have two group of sensors that can provide data in same type (eg.one is accelerometer+gyroscope and another is GPS) , will it run EKF2 separately on two group of sensors or maybe there is some method to fuse two data ?