EKF3 Covariance Update in Simulink – F Matrix Linearization & PS Terms Mapping to P Matrix

ArduCopter Copter 4.6
1

Hi everyone,

I’m trying to build an EKF3 model simulation in Simulink, and I’m currently stuck on the covariance update module—two key parts are unclear to me, especially when cross-referencing the ArduPilot Copter 4.6 source code:

  1. For the state transition matrix F in EKF3: I know F is derived from linearizing the nonlinear state transition model (e.g., attitude, velocity, position dynamics). But I can’t figure out how exactly this linearization is implemented in Copter 4.6’s code—for example, which source files (e.g., EKF3.cpp, ekf3_core.cpp) define the partial derivatives of the state transition function with respect to each state (to form F’s elements)? Are there specific simplifications or approximations used for inertial sensor (IMU) error propagation here?

  2. For the PS intermediate terms in the covariance calculation: In the code, I see a series of PS0 to PS33 variables used to compute the predicted covariance matrix nextP (e.g., nextP[0][0] is a combination of PS0*PS1, -PS11*PS23, etc.). However, I’m struggling to map these PS terms back to the standard EKF covariance update formula (Pk∣k−1​=FPk−1∣k−1​FT+GQGT). Specifically, which PS terms correspond to the FPFT component, and which map to the process noise term GQGT? How do these PS terms translate to individual elements of the P matrix (e.g., does PS14 relate to P[0][10] or another element)?

Has anyone worked on EKF3’s covariance logic in Simulink or dug into the Copter 4.6 code for these details? Any pointers to specific code lines, mathematical derivations, or Simulink implementation tips would be really helpful!

2

I edited the title for brevity and clarity.
The title is not a description, it’s a title.