PX4 EKF2

Flowchart

State vector

State

$$\hat{\mathbf{x}}=\left[\begin{array}{c}C({\hat{q}}_{\mathcal{W}\mathcal{B}})\\ {\hat{\mathbf{v}}}_{\mathcal{B}}\\ {\hat{\mathbf{r}}}_{\mathcal{B}}\\ {}_{\mathcal{B}}{\hat{\mathbf{b}}}^G\\ {}_{\mathcal{B}}{\hat{\mathbf{b}}}^A\\ {}_{\mathcal{W}}\hat{\mathbf{m}}\\ {}_{\mathcal{B}}{\hat{\mathbf{b}}}^M\\ {\hat{\mathbf{v}}}_W\\ {\hat{h}}_T\end{array}\right]$$

• Attitude (quaternion)
• Velocity (NED frame)
• Position (NED frame)
• IMU biases (3 for gyroscope, 3 for accelerometer)
• Earth’s magnetic field vector (NED frame)
• Magnetometer bias (hard/soft iron bias; body frame)
• Wind velocity (North and East components for wind estimation)
• Terrain altitude (height of terrain ie ground above a reference)

State error vector

state_error

$$\Delta \mathbf{x}=\left[\begin{array}{c}\Delta \mathbf{\phi }\\ \Delta {\mathbf{v}}_{\mathcal{B}}\\ \Delta {\mathbf{r}}_{\mathcal{B}}\\ {\Delta }_{\mathcal{B}}{\mathbf{b}}^G\\ {\Delta }_{\mathcal{B}}{\mathbf{b}}^A\\ {\Delta }_{\mathcal{W}}\mathbf{m}\\ {\Delta }_{\mathcal{B}}{\mathbf{b}}^M\\ \Delta {\mathbf{v}}_W\\ \Delta h_T\end{array}\right]$$

• $\Delta \mathbf{\phi }$ is the rotation error in the tangent space in axis angle representation
• $\Delta q={\left[\begin{array}{cccc}1& \frac{\Delta {\phi }_1}{2}& \frac{\Delta {\phi }_2}{2}& \frac{\Delta {\phi }_3}{2}\end{array}\right]}^{\top }$ is the corresponding error quaternion, assuming the rotation $\Delta \phi$ is small (see small angle approximation)

True state vector

state_t

$$\mathbf{x}=\left[\begin{array}{c}C(\Delta q)C({\hat{q}}_{\mathcal{W}\mathcal{B}})\\ (\hat{\mathbf{x}}+\Delta \mathbf{x}{)}_{\text{1:}}\end{array}\right]$$

Noise vector

noise

$$\mathbf{n}=\left[\begin{array}{c}{\mathbf{n}}^A\\ {\mathbf{n}}^G\end{array}\right]$$

Implementation details

Error state formulation

• To improve stability
• Instead of estimating full state errors directly, small error corrections to a nominal state are tracked

Prediction step

• Uses IMU data only

Code implementation

1. predict_covariance

New symbols	Meaning	Variable name
${\tilde{\mathbf{u}}}_k=\left[\begin{array}{c}\tilde{\mathbf{a}}\tilde{\mathbf{\omega }}\end{array}\right]$	raw IMU specific force and angular-rate samples	accel, gyro
${\hat{\mathbf{b}}}_k=\left[\begin{array}{c}{}_{\mathcal{B}}{\hat{\mathbf{b}}}^A{}_{\mathcal{B}}{\hat{\mathbf{b}}}^G\end{array}\right]$	estimated IMU biases	state["accel_bias"], state["gyro_bias"]
${\sigma }_a^2$, ${\sigma }_{\omega }^2$	element-wise accel / gyro variances	accel_var, gyro_var
${\mathbf{A}}_k=\frac{\partial \Delta {\mathbf{x}}_{k+1}}{\partial \Delta {\mathbf{x}}_k}$	Discrete-time state-transition Jacobian in the error-state space (how the error evolves from step $k$ to $k+1$)	A
${\mathbf{G}}_k=\frac{\partial \Delta {\mathbf{x}}_{k+1}}{\partial {\mathbf{n}}_k}$	Process-noise Jacobian (how the IMU noise influences the error dynamics during the step)	G

Nominal state propagation

$${\hat{\mathbf{x}}}_{k+1}=f({\hat{\mathbf{x}}}_k,{\tilde{\mathbf{u}}}_k-{\hat{\mathbf{b}}}_k)$$

Error-state kinematics

$$\Delta {\mathbf{x}}_{k+1}={\mathbf{A}}_k\Delta {\mathbf{x}}_k+{\mathbf{G}}_k{\mathbf{n}}_k$$

Covariance propagation

$${\mathbf{P}}_{k+1}={\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^{\mathsf{T}}+{\mathbf{G}}_k\underset{{\mathbf{Q}}_k}{\underbrace{\mathrm{diag}({\sigma }_a^2{\mathbf{I}}_3,{\sigma }_{\omega }^2{\mathbf{I}}_3)}}{\mathbf{G}}_k^{\mathsf{T}}$$

---

2. compute_airspeed_innov_and_innov_var

Wind-compensated relative velocity

$${\mathbf{v}}_{\mathrm{rel}}={\hat{\mathbf{v}}}_{\mathcal{B}}-\left[\begin{array}{c}{\hat{v}}_{W,x}\\ {\hat{v}}_{W,y}\\ 0\end{array}\right]$$

Predicted airspeed

$${\hat{v}}_a=\Vert {\mathbf{v}}_{\mathrm{rel}}\Vert$$

Innovation

$${\tilde{v}}_a={\hat{v}}_a-v_a^{\text{meas}}$$

Jacobian

$${\mathbf{H}}_{v_a}={\frac{\partial {\hat{v}}_a}{\partial \Delta \mathbf{x}}|}_{\Delta \mathbf{x}=0}$$

Innovation variance $S_{v_a}={\mathbf{H}}_{v_a}\mathbf{P}{\mathbf{H}}_{v_a}^{\mathsf{T}}+R_{v_a}$

New symbol	Variable name
$v_a^{\text{meas}}$	airspeed
$R_{v_a}$	R
${\hat{v}}_{W,x},{\hat{v}}_{W,y}$	state["wind_vel"][0], [1]

---

3. compute_airspeed_h_and_k

Kva=PHvaT Sva−1,Hva as above.

$${\mathbf{K}}_{v_a}=\mathbf{P}{\mathbf{H}}_{v_a}^{\mathsf{T}}{S}_{v_a}^{-1},{\mathbf{H}}_{v_a}\text{ as above.}$$

(No new symbols.)

---

4. compute_wind_init_and_cov_from_airspeed

$$\begin{array}{rl}& \text{Solve}\left[\begin{array}{c}{\hat{v}}_{W,x}\\ {\hat{v}}_{W,y}\end{array}\right]=\left[\begin{array}{c}{v}_N-v_a\cos (\psi +\beta )\\ v_E-v_a\sin (\psi +\beta )\end{array}\right],\\ & \text{with}\beta \approx 0⟹{\hat{\mathbf{v}}}_W=\left[\begin{array}{c}{v}_N-v_a\cos \psi \\ v_E-v_a\sin \psi \end{array}\right].\end{array}$$

The covariance is found with first-order error propagation
${\mathbf{P}}_W=\mathbf{J},\mathbf{R},{\mathbf{J}}^{\mathsf{T}}$, where $\mathbf{J}$ is the Jacobian of the above mapping and $\mathbf{R}$ the diagonal matrix of the supplied variances.

New / mapped symbols

symbol	python variable
$v_N,v_E$	v_local[0], v_local[1] (aircraft velocity in local-NED)
$\psi$	heading
$\beta$	explicit symbolic sideslip
$\mathbf{R}$	constructed from v_var, heading_var, sideslip_var, airspeed_var

---

5. compute_wind_init_and_cov_from_wind_speed_and_direction

$${\hat{\mathbf{v}}}_W=\left[\begin{array}{c}{v}_W\cos {\psi }_W\\ v_W\sin {\psi }_W\end{array}\right],{\mathbf{P}}_W=\mathbf{H}\mathrm{diag}({\sigma }_{v_W}^2,{\sigma }_{{\psi }_W}^2){\mathbf{H}}^{\mathsf{T}},$$

with
$\mathbf{H}=\frac{\partial {\hat{\mathbf{v}}}_W}{\partial ,(v_W,{\psi }_W)}$.

symbol	python variable
$v_W$	wind_speed
${\psi }_W$	wind_direction
${\sigma }_{v_W}^2$, ${\sigma }_{{\psi }_W}^2$	wind_speed_var, wind_direction_var

---

6. predict_sideslip & compute_sideslip_*

$$\begin{array}{rl}& {\beta }_{\text{pred}}=\frac{v_{\mathrm{rel},y}}{v_{\mathrm{rel},x}}\approx \tan \beta (\text{small angle}),\\ & \tilde{\beta }={\beta }_{\text{pred}}-0,\\ & {\mathbf{H}}_{\beta }={\frac{\partial {\beta }_{\text{pred}}}{\partial \Delta \mathbf{x}}|}_{\Delta \mathbf{x}=0},S_{\beta }={\mathbf{H}}_{\beta }\mathbf{P}{\mathbf{H}}_{\beta }^{\mathsf{T}}+R_{\beta },\\ & {\mathbf{K}}_{\beta }=\mathbf{P}{\mathbf{H}}_{\beta }^{\mathsf{T}}{S}_{\beta }^{-1}.\end{array}$$

symbol	python variable
$R_{\beta }$	R

---

7. predict_vel_body & body-velocity helpers

$${\mathbf{v}}_{\mathcal{B}}=C({\hat{q}}_{\mathcal{W}\mathcal{B}}{)}^{-1}{\hat{\mathbf{v}}}_{\mathcal{B}}^W.$$

For each axis $i\in x,y,z$:

$$\begin{array}{rl}& {\tilde{v}}_{\mathcal{B},i}={\hat{v}}_{\mathcal{B},i}-v_{\mathcal{B},i}^{\text{meas}},\\ & {\mathbf{H}}_i={\frac{\partial {\hat{v}}_{\mathcal{B},i}}{\partial \Delta \mathbf{x}}|}_0,S_i={\mathbf{H}}_i\mathbf{P}{\mathbf{H}}_i^{\mathsf{T}}+R_i.\end{array}$$

symbol	python variable
$v_{\mathcal{B},i}^{\text{meas}}$	components of an external body-frame velocity sensor (if present) or zeros when only $S$ is requested
$R_i$	entries of vector R (or scalar for *_y/z variants)

---

8. predict_mag_body & magnetometer helpers

$${\hat{\mathbf{m}}}_{\mathcal{B}}=C({\hat{q}}_{\mathcal{W}\mathcal{B}}{)}^{-1}{}_{\mathcal{W}}\hat{\mathbf{m}}+{}_{\mathcal{B}}{\hat{\mathbf{b}}}^M.$$

Each component innovation ${\tilde{m}}_{\mathcal{B},i}$, jacobian ${\mathbf{H}}_i$, variance $S_i$ (and Kalman gain if required) follow the same pattern as above.

symbol	python variable
${}_{\mathcal{B}}{\hat{\mathbf{b}}}^M$	state["mag_B"]
${}_{\mathcal{W}}\hat{\mathbf{m}}$	state["mag_I"]
$R_m$	R

---

9. compute_yaw_innov_var_and_h

$$\begin{array}{rl}& \tilde{\psi }=2\Delta q_z,{\mathbf{H}}_{\psi }={\frac{\partial (2\Delta q_z)}{\partial \Delta \mathbf{x}}|}_0,\\ & S_{\psi }={\mathbf{H}}_{\psi }\mathbf{P}{\mathbf{H}}_{\psi }^{\mathsf{T}}+R_{\psi }.\end{array}$$

symbol	python variable
$R_{\psi }$	R

---

10. compute_mag_declination_pred_innov_var_and_h

$${\hat{\psi }}_D=atan2({}_{\mathcal{W}}{\hat{m}}_y,{}_{\mathcal{W}}{\hat{m}}_x),S_{{\psi }_D}={\mathbf{H}}_{{\psi }_D}\mathbf{P}{\mathbf{H}}_{{\psi }_D}^{\mathsf{T}}+R_{{\psi }_D}.$$

---

11. predict_hagl, optical-flow & HAGL helpers

$${\hat{h}}_{\text{AGL}}={\hat{h}}_T-{\hat{r}}_{\mathcal{B},z}.$$

Optical-flow LOS-rate prediction (sensor frame):

$$\begin{array}{rl}{\mathbf{\omega }}_{\text{LOS}}& =\frac{1}{|{\hat{h}}_{\text{AGL}}|}\left[\begin{array}{c}{\hat{v}}_{\text{rel},y}\\ -{\hat{v}}_{\text{rel},x}\end{array}\right]C({\hat{q}}_{\mathcal{W}\mathcal{B}}{)}_{3,3}.\end{array}$$

Each axis uses the usual Kalman innovation/variance formulas.

---

12. compute_gnss_yaw_pred_innov_var_and_h

$$\begin{array}{rl}& {\mathbf{a}}_B=\left[\begin{array}{c}\cos {\delta }_a\\ \sin {\delta }_a\\ 0\end{array}\right],{\mathbf{a}}_W=C({\hat{q}}_{\mathcal{W}\mathcal{B}}){\mathbf{a}}_B,\\ & {\hat{\psi }}_{\text{ant}}=atan2(a_{W,y},a_{W,x}),S_{\text{ant}}={\mathbf{H}}_{\text{ant}}\mathbf{P}{\mathbf{H}}_{\text{ant}}^{\mathsf{T}}+R_{\text{ant}}.\end{array}$$

symbol	python variable
${\delta }_a$	antenna_yaw_offset
$R_{\text{ant}}$	R

---

13. predict_drag & drag helpers

$$\begin{array}{rl}{\mathbf{F}}_{D,xy}& =-\frac{1}{2}\rho C_D\Vert {\mathbf{v}}_{\mathrm{rel}}\Vert {\mathbf{v}}_{\mathrm{rel},xy}-C_M{\mathbf{v}}_{\mathrm{rel},xy}.\end{array}$$

---

14. predict_gravity_direction & gravity helpers

$${\hat{\mathbf{g}}}_{\mathcal{B}}=C({\hat{q}}_{\mathcal{W}\mathcal{B}}{)}^{-1}\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right].$$

Each component innovation and variance again follows the generic pattern.

---

How to read these blocks

1. Every measurement helper shares the same EKF algebra:

   • predicted measurement $h(\hat{\mathbf{x}})$

   • innovation $\tilde{y}=h(\hat{\mathbf{x}})-y$

   • Jacobian $\mathbf{H}$, innovation variance $S$, gain $\mathbf{K}$.

2. All quaternion updates use the small-angle approximation
   $\delta \mathbf{\phi }\simeq 2,\mathbf{vec}(\Delta q)$,
   matching the theta component of state_error.

3. All Jacobians are built exactly as in the script via SymForce, but their analytical expressions are those shown above.