Predict covariance in EKF-GSF estimator

[M_O]

In the function EKFGSF_yaw::predictEKF we used the AHRS filter to predict the current orientation which gave us as output of this filter the rotation matrix body to earth, we used the EKF to predict the yaw angle (orientation over down axis of body)by running the line _ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R);

After the the predicted yaw is stored in the EKF-GSF estimator, the state vector X in this estimator will be
X=[V_(k-1)N, V(k-1)E, yaw_angle_k] with V(k-1)N velocity in N axis previous instant k-1 V(k-1)_E velocity in E axis previous instant k-1, yaw_angle_k the yaw in down body axis in this instant

we project the delta velocity to the NE rotated with yaw angle we call it XY frame, and projected the delta angle from body to the down axis D of NED frame, the variables are called respectively “dvx”, “dvy”, “daz”.

we pass the state vector X, it’s variances, the dvx, dvy, daz, the covariance matrix and the to the autogenerated function YawEstPredictCovariance (generated from the script derivation_yaw_estimator.py). After I went to check how the covariance is predicted in script derivation_yaw_estimator.py, I found in the function def yaw_est_predict_covariance that to calculate the velocity of the predicted state we rotate the “dvz” with a rotation that bring it back to the NED so that we get the predicted velocity in NED.

        Questions:

Since the rotation R in state we uses is already the current orientation so why do we rotate it once more with in state_pred and state_t_pred even if we already did that in the AHRS? since velocity and orientation are in different timestamp it’s logic to update the velocity in prediction but why updating the already updated angle?

Why the delta angle passed to the function “daz” is the rotation of delta angle projected to the down axis of NED?

In the comment its written that “Derive the covariance prediction equations Error growth in the inertial solution is assumed to be driven by ‘noise’ in the delta angles and velocities, after bias effects have been removed”, but the bias is not corrected, not in the delta angle nor the delta velocity since both the one used are the one we get from the IMU_downsampled sample which is not bias corrected?

Why when performing the symbolic derivation to get F and G you considered zero_state_error and zero_noise instead of another value such bias*dt?

why the fusion and control(disturbance) influence matrix and matrix are calcultad with F= jacobian(state_error_predicted, error) and G= jacobian(state_error_predicted,noise) when for the EKF their defined as F=jacobian(state_predicted,state) and G= jacobian(state_predicted, [delta_velocity,delta_angle]) with delta_velocity and delta_angle the measurement not corrected from bias as in EKF.pdf - Google Drive, which is coherent with what was programmed in the script GenerateEquations24.m in the archived repository PX4-ECL

[M_O]

@bresch , @Paul_Riseborough I doubt anyone can save the day but you, I’ve been struggling to understand the logic for days but still no clue.

[bresch]

Hi, I’m sorry for the really late answer, I had to go through the code again to get your answers as I didn’t write most of it.

@M_O

Why the delta angle passed to the function “daz” is the rotation of delta angle projected to the down axis of NED?

Because the EKF runs in 2D (vel_North, vel_East and Yaw); in its model we cannot transform from the 3D body to NED. We then need the yaw rate (daz / dt) to predict the covariance.

@M_O

Since the rotation R in state we uses is already the current orientation so why do we rotate it once more with in state_pred and state_t_pred even if we already did that in the AHRS? since velocity and orientation are in different timestamp it’s logic to update the velocity in prediction but why updating the already updated angle?

That’s actually true, IMO to be exact we should first remove the delta yaw from the current yaw to compute dvx, dvz and predict the covariance as the covariance prediction should happen before the state prediction.

@M_O

n the comment its written that “Derive the covariance prediction equations Error growth in the inertial solution is assumed to be driven by ‘noise’ in the delta angles and velocities, after bias effects have been removed”, but the bias is not corrected, not in the delta angle nor the delta velocity since both the one used are the one we get from the IMU_downsampled sample which is not bias corrected?

I don’t know if that was lost at some point but it’s correct that the predict function gets uncorrected delta angle and delta velocities. Not sure if it’s best to use the bias estimates from the -potentially corrupted- main EKF or use the raw values and hope that the biases are small enough to not affect the emergency estimator.

@M_O

Why when performing the symbolic derivation to get F and G you considered zero_state_error and zero_noise instead of another value such bias*dt?

The “zero_noise” and “zero_state_error” are used to remove the high order terms that would appear in the jacobian: we basically assume that error^2 = 0

@M_O

why the fusion and control(disturbance) influence matrix and matrix are calcultad with F= jacobian(state_error_predicted, error) and G= jacobian(state_error_predicted,noise) when for the EKF their defined as F=jacobian(state_predicted,state) and G= jacobian(state_predicted, [delta_velocity,delta_angle]) with delta_velocity and delta_angle the measurement not corrected from bias as in EKF.pdf - Google Drive , which is coherent with what was programmed in the script GenerateEquations24.m in the archived repository PX4-ECL

The derivation is done using an error-state formulation, it’s the same now for the main EKF: the evolution of the state covariance is based on the evolution of the error state rather than the evolution of the state.