EKF3 position and velocity noise parameters

bobzwik · March 1, 2023, 2:33pm

Hey everyone, I had a few questions regarding the position and velocity noise parameters for the EKF3. Specifically:

According to the docs, EK3_VELNE_M_NSE and EK3_VELD_M_NSE are the lower limits to the velocity accuracy reported by the GPS. I thankfully noticed these parameters and lowered them to 0.05 m/s, since I’m using F9P RTK.

The wording for EK3_POSNE_M_NSE is different than for the 2 others.

This sets the GPS horizontal position observation noise

What does that mean? Is this equivalent to the position accuracy reported by the GPS? Or is it also a lower limit to that accuracy? Does the EKF3 not use the accuracy reported by the GPS? The minimum value for this parameter is 10cm, while I’ve tested my F9P RTK to wander 6 mm when stationary. In flight (with speeds of 0-70 km/h) the GPS reports an HAcc of 1cm, and a radius of uncertainty (HDOP*theoretical CEP) of 8 mm. With this is mind, should I be lowering the EK3_POSNE_M_NSE to 1 cm (past the lower limit of the param)?

My main question though, is why isn’t there a similar parameter for the vertical position noise/accuracy? Something like EK3_POSD_M_NSE. Does the EKF3 simply use the reported vertical accuracy by the GPS (also 1 cm), or is there a accuracy value hardcoded in the code? I’ve noticed that there is a parameter EK3_ALT_M_NSE, but that seems to be the noise value of the barometer, which is disabled in my case.

How do I make sure that the EKF3 knows that my GPS vertical position is accurate to the centimeter?

bobzwik · March 5, 2023, 2:26am

@Paul_Riseborough @rmackay9 @peterbarker @tridge

Sorry for the tag, but I’m hoping one of you might have some insight on these parameters. Your help will be much appreciated.

John

amilcarlucas · March 5, 2023, 12:10pm

That is not good net-etiquette, please do not tag people like that.

Provide as much correct and accurate information as possible, that is the best way of increasing the chance of getting an answer.

AFAIK the accuracy reported by the GNSS receiver is only used for 2 receiver blending. The EKF fully ignores the GNSS reported accuracy when fusing in the GNSS position data.

bobzwik · March 6, 2023, 9:38pm

What I’ve figured out

This is what I’ve managed to figure out with my limited knowledge of C++.

The parameters of interest are stored in the following floats (found here):

VELNE_M_NSE → gpsHorizVelNoise
VELD_M_NSE → gpsVertVelNoise
POSNE_M_NSE → gpsHorizPosNoise

In NavEKF3_core::FuseVelPosNED() (found here), we create a vector for measurement variances.

Vector6 R_OBS; // Measurement variances used for fusion

Which are the measurement variances for, in order, VelN, VelE, VelD, PosN, PosE, PosD.

This is what is assigned to R_OBS:

R_OBS[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
R_OBS[1] = R_OBS[0];

R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
R_OBS[4] = R_OBS[3];
R_OBS[5] = posDownObsNoise;

So here, the variances for VelN and VelE is gpsSpdAccuracy constrained between gpsHorizVelNoise (our parameter VELNE_M_NSE) and 50 m/s. The variance for VelD is gpsSpdAccuracy constrained between gpsVertVelNoise (VELD_M_NSE) and 50 m/s.

Horizontal position variance is gpsPosAccuracy constrained between gpsHorizPosNoise (POSNE_M_NSE) and 100 m. Vertical position variance is simply posDownObsNoise.

posDownObsNoise is obtained here, and is equal to gpsHgtAccuracy constrained between 1.5*gpsHorizPosNoise and 100 m.

As far as I can tell, gpsPosAccuracy, gpsHgtAccuracy and gpsSpdAccuracy come from NavEKF3_core::readGpsData(). For example:

if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
        gpsSpdAccuracy = 0.0f;
    } else {
        gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
        gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
        gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise);
    }

This takes the GPS’s reported speed accuracy and puts it in gpsSpdAccRaw

The speed accuracy gpsSpdAccuracy used by NavEKF3_core::FuseVelPosNED() is gpsSpdAccRaw, limited to a minimum of gpsHorizVelNoise (VELNE_M_NSE) and a maximum of 50m/s.

The horizontal and vertical GPS position accuracies are similarly treated. The horizontal position accuracy gpsPosAccuracy is limited to a minimum of gpsHorizPosNoise (POSNE_M_NSE) and a maximum of 100 m, while the vertical position accuracy gpsHgtAccuracy is limited to a minimum of 1.5*gpsHorizPosNoise (POSNE_M_NSE) and a maximum of 100 m.

Source of GPS reported accuracies:

I use a UAVCAN GPS, so I guess this is where the speed accuracy are retrieved from the GPS

interim_state.speed_accuracy = sqrtf((cb.msg->covariance[3] + cb.msg->covariance[4] + cb.msg->covariance[5])/3);

Position accuracies (horizontal and vertical) are also obtained in the same function.

Conclusion

In summary, the velocity accuracy and the horizontal and vertical position accuracies reported by the GPS are used in the EKF3, and are twice constrained to the same limit?

R_OBS[3] and R_OBS[4] (NE position variance) constrained twice at a minimum of POSNE_M_NSE.
R_OBS[5] (D position variance) constrained twice at a minimum of 1.5*POSNE_M_NSE.

R_OBS[0] and R_OBS[1] (NE velocity variance) constrained twice at a minimum of VELNE_M_NSE.
R_OBS[2] (D velocity variance) constrained once at a minimum of VELNE_M_NSE and once at a minimum of VELD_M_NSE.

Are my conclusions correct?

bobzwik · March 6, 2023, 9:41pm

This brings more questions:

In NavEKF3_core::CovarianceInit() (here), a covariance matrix is initialized, and the parameters of interest are used. The PosD covariance used is the parameter for barometer noise (ALT_M_NSE):

// velocities
P[4][4]   = sq(frontend->_gpsHorizVelNoise);
P[5][5]   = P[4][4];
P[6][6]   = sq(frontend->_gpsVertVelNoise);
// positions
P[7][7]   = sq(frontend->_gpsHorizPosNoise);
P[8][8]   = P[7][7];
P[9][9]   = sq(frontend->_baroAltNoise);

I found that P[4][4] and P[5][5] are reset in NavEKF3_core::ResetVelocity() (here) to

P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));

P[7][7] and P[8][8] are reset in NavEKF3_core::ResetPosition() (here) to

P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));

P[9][9] is set in NavEKF3_core::ResetHeight() (here) to

P[9][9] = posDownObsNoise;

But for the vertical velocity variance, P[6][6], it only shows up in NavEKF3_core::ConstrainVariances() (here) as

P[6][6] = sq(frontend->_gpsVertVelNoise);

and in NavEKF3_core::ResetHeight() (here) as

P[6][6] = sq(frontend->_gpsVertVelNoise);

Are these functions (ResetVelocity(), ResetPosition(), ResetHeight() and ConstrainVariances()) continuously being called in flight? If so, does that means that the speed accuracy and NE position accuracy reported by the GPS are being used in this covariance matrix, but not the GPS’s vertical position accuracy? Why?

I also found out that this P matrix is logged in XKV. I had a look at my logs, and I am now even more perplexed.

My parameters were:
VELNE_M_NSE = 0.3 m/s
VELD_M_NSE = 0.5 m/s
POSNE_M_NSE = 0.5 m

So I shouldn’t expect values lower than their square (0.09 and 0.25) in the XKV log, right? Yet, here are the average values in the XKV log:

P[4][4] = 0.009 m/s
P[5][5] = 0.009 m/s
P[6][6] = 0.006 m/s

P[7][7] = 0.027 m
P[8][8] = 0.027 m
P[9][9] = 0.047 m

I was wondering how is this possible, if these should be no less than 0.09 and 0.25, as they are constrained by the x_NSE parameters.

And if the variances are so good, how am I often reaching terrible vertical innovations (differences between predicted states and observations) on my primary EKF lane? Some times all EKF lanes nicely match the vertical position and vertical velocity reported by the GPS, sometimes there are differences of up to 1.2 m and 0.8 m/s.

This is the issue I’m trying to solve. I’m using a RTK GPS because I need my drone to have an accurate estimate of it’s position. Here is an example of a problematic flight. The following figure shows the reported GPS altitude (minus EKF origin altitude) and vertical velocity, compared to the 3 EKF estimations, and the position and velocity used by the position controller (PSCD), basically telling me which EKF lane is being used. Horizontally, the drone is traveling at 20 km/h and slowly reaches hover at the dotted vertical line, and then gradually accelerates to reach 60 km/h at the end of the highlighted red zone.

The vertical innovations are large, mostly in lane 0 and lane 1:

Yet the variances are tiny:

Vibrations are low (under 5 m/s2 in all axes on all IMU’s) and accelerations are low.

I know I’m missing crucial information into understanding what is going on, and how the x_NSE parameters can affect the state estimation. What is the difference between observation variances R_OBS I found in my last reply and the variances used in the covariance matrix P?

I am at heart a mechanical engineer, and would need some help understanding this.

Again, sorry for the tag. I am only trying to reach the best minds that can help me out.

Also, please let me know if this is a discussion best had over on Discord.