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