PX4 EKF Code Review

A mathematical review of PX4’s implementation of the Extended Kalman Filter(EKF) algorithm.

\renewcommand{\bold}{\mathbf} \renewcommand{\R}{\mathbb{R}}

Kalman Assumption

  • xk\bold x_k state vector
  • uk\bold u_k control vector
  • zk\bold z_k measurement vector

xk=Akxk−1+Bkuk+wk\bold{x}_k = \bold{A}_k \bold{x}_{k-1} + \bold{B}_k \bold{u}_k + \bold{w}_k

  • Assume xk\bold{x}_k is a normally distributed random variable, whose mean is a linear function of xk−1\bold{x}_{k-1} and uk\bold{u}_k.

zk=Ckxk+vk\bold{z}_k = \bold{C}_k \bold{x}_k + \bold{v}_k

  • Assume zk\bold z_k is a normally distributed random variable, whose mean is a linear function of xk\bold{x}_k.

Multivariate Normal Distribution PDF

p(x)=det⁡(2πΣ)−1/2exp⁡{−12(x−μ)TΣ−1(x−μ)}p(\bold{x}) = \det(2\pi\bold\Sigma)^{-1/2} \exp\{-\dfrac{1}{2}(\bold x-\boldsymbol \mu)^T\bold\Sigma^{-1}(\bold x-\boldsymbol \mu)\}

Transitional Probability

Define μk=Akxk−1+Bkuk\boldsymbol \mu_k = \bold{A}_k \bold{x}_{k-1} + \bold{B}_k \bold{u}_k which is the expectation of xk\bold x_k. Define Qk\bold{Q}_k to be the variance of xk\bold x_k.

xk∼N(μk,Qk)\bold x_k \sim \mathcal N(\boldsymbol \mu_k, \bold Q_k)

In fact it is the covariance matrix (natural generalization to higher dimensions of the 1-dimensional variance)

p(xk∣uk,xk−1)=∣2πQk∣−1/2exp⁡[−12(xk−μk)TQk−1(xk−μk)]p(\bold{x}_k|\bold{u}_k, \bold{x}_{k-1}) = |2\pi \bold Q_k|^{-1/2} \exp[-\dfrac{1}{2}(\bold x_k-\boldsymbol \mu_k)^T \bold Q_k^{-1}(\bold x_k-\boldsymbol \mu_k)]

Similarily, define ϕk=Ckxk\boldsymbol \phi_k = \bold C_k \bold x_k which is the expectation of zk\bold z_k. Define Rk\bold R_k to be the variance of zk\bold z_k.

zk∼N(ϕk,Rk)\bold z_k \sim \mathcal N(\boldsymbol \phi_k, \bold R_k)

p(zk∣xk)=∣2πRk∣−1/2exp⁡[−12(zk−ϕk)TRk−1(zk−ϕk)]p(\bold z_k|\bold x_k) = |2\pi \bold R_k|^{-1/2} \exp[-\dfrac{1}{2}(\bold z_k-\boldsymbol\phi_k)^T \bold R_k^{-1}(\bold z_k-\boldsymbol\phi_k)]

Kalman Algorithm

  • x^k∣k−1\hat{\bold x}_{k|k-1}
    • priori estimate of state
  • x^k∣k\hat{\bold x}_{k|k}
    • posteriori estimate of state
  • Pk∣k−1\bold P_{k|k-1}
    • priori estimate of variance
  • Pk∣k\bold P_{k|k}
    • posteriori estimate of variance

Algorithm Kalman Filter(x^k−1∣k−1,Pk−1∣k−1,uk,zk):x^k∣k−1=Akx^k−1∣k−1+BkukPk∣k−1=AkPk−1∣k−1AkT+QkKk=Pk∣k−1CkT(CkPk∣k−1CkT+Rk)−1x^k∣k=x^k∣k−1+Kk(zk−Ckx^k∣k−1)Pk∣k=(I−KkCk)Pk∣k−1return x^k∣k,Pk∣k\begin{aligned} & \textbf{Algorithm Kalman Filter} (\hat{\bold x}_{k-1|k-1}, \bold P_{k-1|k-1}, \bold u_k, \bold z_k): \\ &\qquad \hat{\bold x}_{k|k-1} = \bold A_k \hat{\bold x}_{k-1|k-1} + \bold B_k \bold u_k \\ &\qquad \bold P_{k|k-1} = \bold A_k \bold P_{k-1|k-1} \bold A_k^{T} + \bold Q_k \\ &\qquad \bold K_k = \bold P_{k|k-1} \bold C_k^T(\bold C_k \bold P_{k|k-1} \bold C_k^T + \bold R_k)^{-1}\\ &\qquad \hat{\bold x}_{k|k} = \hat{\bold x}_{k|k-1} + \bold K_k(\bold z_k - \bold C_k \hat{\bold x}_{k|k-1}) \\ &\qquad \bold P_{k|k} = (\bold I - \bold K_k \bold C_k) \bold P_{k|k-1} \\ & \textbf{return } \hat{\bold x}_{k|k}, \bold P_{k|k} \end{aligned}

  1. Priori estimation of state using model mean

  2. Priori estimation of variance accordingly

  3. Observe measurement, calculate the measurement residual

    • The model differs from measurement

    • (CkPk∣k−1CkT+Rk)−1(\bold C_k \bold P_{k|k-1} \bold C_k^T + \bold R_k)^{-1} is the variance of the residual

    • Using the variance to calculate the Kalman gain (how much we take additional measurement into account)

      • Large Kalman gain → measurement
      • Small Kalman gain → model

  4. Posteriori estimate of state using Kalman gain

    • (zk−Ckx^k∣k−1)(\bold z_k - \bold C_k \hat{\bold x}_{k|k-1}) is the mean of the residual

  5. Posteriori estimate of variance using Kalman gain

Extended Kalman Algorithms

  • xk\bold x_k state vector
  • uk\bold u_k control vector
  • zk\bold z_k measurement vector

xk=g(uk,xk−1)+wkzk=h(xk)+vk\bold{x}_k = g(\bold{u}_k, \bold{x}_{k-1}) + \bold{w}_k \\ \bold{z}_k = h(\bold{x}_k) + \bold{v}_k \\

g(x)g(x) and h(x)h(x) can be nonlinear. Expand g(x)g(x) and h(x)h(x) using Taylor expansion:

g(uk,xk−1)≈g(uk,μk−1)+g′(uk,μk−1)(xk−1−μk−1):=g(uk,μk−1)+Gk(xk−1−μk−1)h(xk)≈h(μk)+h′(μk)(xk−μk):=h(μk)+Hk(xk−μk)\begin{aligned} g(\bold u_k, \bold x_{k-1}) &\approx g(\bold u_k, \boldsymbol \mu_{k-1}) + g'(\bold u_k, \boldsymbol \mu_{k-1})(\bold x_{k-1}- \boldsymbol \mu_{k-1}) \\ &:= g(\bold u_k, \boldsymbol \mu_{k-1}) + \bold G_k(\bold x_{k-1}- \boldsymbol \mu_{k-1}) \\ h(\bold x_{k}) &\approx h(\boldsymbol \mu_{k}) + h'(\boldsymbol \mu_{k})(\bold x_{k} - \boldsymbol \mu_{k}) \\ &:= h(\boldsymbol \mu_{k}) + \bold H_k(\bold x_{k} - \boldsymbol \mu_{k}) \\ \end{aligned}

G\bold G and H\bold H correspond to A\bold A and C\bold C in Kalman Filter.

Algorithm Extended Kalman Filter(x^k−1∣k−1,Pk−1∣k−1,uk,zk):x^k∣k−1=g(uk,x^k−1∣k−1)Pk∣k−1=GkPk−1∣k−1GkT+QkKk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1x^k∣k=x^k∣k−1+Kk(zk−h(x^k∣k−1))Pk∣k=(I−KkHk)Pk∣k−1return x^k∣k,Pk∣k\begin{aligned} & \textbf{Algorithm Extended Kalman Filter} (\hat{\bold x}_{k-1|k-1}, \bold P_{k-1|k-1}, \bold u_k, \bold z_k): \\ &\qquad \hat{\bold x}_{k|k-1} = g(\bold u_k, \hat{\bold x}_{k-1|k-1}) \\ &\qquad \bold P_{k|k-1} = \bold G_k \bold P_{k-1|k-1} \bold G_k^{T} + \bold Q_k \\ &\qquad \bold K_k = \bold P_{k|k-1} \bold H_k^T(\bold H_k \bold P_{k|k-1} \bold H_k^T + \bold R_k)^{-1}\\ &\qquad \hat{\bold x}_{k|k} = \hat{\bold x}_{k|k-1} + \bold K_k(\bold z_k - h(\hat{\bold x}_{k|k-1})) \\ &\qquad \bold P_{k|k} = (\bold I - \bold K_k \bold H_k) \bold P_{k|k-1} \\ & \textbf{return } \hat{\bold x}_{k|k}, \bold P_{k|k} \end{aligned}

Example

State Vector

x=[ωω˙rarm]\bold x = \begin{bmatrix} \boldsymbol \omega \\ \dot{\boldsymbol \omega} \\ \bold r_a \\ \bold r_m \end{bmatrix}

  • x∈R12×1\bold x \in \R^{12\times1}
  • ω=[ωx,ωy,ωz]T\boldsymbol\omega = [\omega_x, \omega_y, \omega_z]^T, real angular velocity
  • ω˙=[ω˙x,ω˙y,ω˙z]T\dot{\boldsymbol\omega} = [\dot\omega_x, \dot\omega_y,\dot \omega_z]^T, real angular acceleration
  • ra=[rax,ray,raz]T\bold r_a = [r_{ax}, r_{ay}, r_{az}]^T, real acceleration
  • rm=[rmx,rmy,rmz]T\bold r_m = [r_{mx}, r_{my}, r_{mz}]^T, real magnetic field

Measurement vector

z=[ωˉrˉarˉm]\bold z = \begin{bmatrix} \bar{\boldsymbol\omega} \\ \bar{\bold r}_a \\ \bar{\bold r}_m \end{bmatrix}

  • z∈R9×1\bold z \in \R^{9 \times 1}
  • ωˉ\bar{\boldsymbol\omega}, measured angular velocity from gyroscope
  • rˉa\bar{\bold r}_a, measured acceleration from accelerometer
  • rˉm\bar{\bold r}_m, measured magnetic field from magnetometer

Prediction Model

xk+1=g(xk)+wk≈[ωk+ω˙kΔtω˙kra,k+ω~kra,kΔtrm,k+ω~krm,kΔt]+[wωkwω˙kwra,kwrm,k]\bold x_{k+1} = g(\bold x_k)+ \bold w_k \approx \begin{bmatrix} \boldsymbol \omega_k + \dot{\boldsymbol \omega}_k \Delta t \\ \dot{\boldsymbol \omega}_k \\ \bold r_{a,k} + \tilde{\boldsymbol\omega}_k \bold r_{a,k} \Delta t \\ \bold r_{m,k} + \tilde{\boldsymbol\omega}_k \bold r_{m,k} \Delta t \\ \end{bmatrix} + \begin{bmatrix} \bold w_{\omega_k} \\ \bold w_{\dot\omega_k} \\ \bold w_{r_{a, k}} \\ \bold w_{r_{m, k}} \\ \end{bmatrix}

wk\bold w_k is Guassian noise. ω~k\tilde{\boldsymbol\omega}_k is the direction cosine matrix(DCM)

ω~k=[0ωz−ωy−ωz0ωxωy−ωx0]ω~kra,k=[ray,kωz−raz,kωy−rax,kωz+raz,kωxrax,kωy−ray,kωx]∂ω~kra,k∂ωk=[0−raz,kray,kraz,k0−rax,k−ray,krax,k0]:=−r~a,k\tilde{\boldsymbol\omega}_k = \begin{bmatrix} 0 & \omega_z & -\omega_y\\ -\omega_z & 0 & \omega_x \\ \omega_y & -\omega_x & 0 \end{bmatrix} \\ \tilde{\boldsymbol\omega}_k \bold r_{a,k} = \begin{bmatrix} r_{ay,k}\omega_z - r_{az,k}\omega_y\\ -r_{ax,k}\omega_z + r_{az,k}\omega_x \\ r_{ax,k}\omega_y - r_{ay,k} \omega_x \end{bmatrix}\\ \dfrac{\partial \tilde{\boldsymbol\omega}_k \bold r_{a,k}}{\partial \boldsymbol\omega_k} = \begin{bmatrix} 0 & -r_{az,k} & r_{ay,k}\\ r_{az,k} & 0 & -r_{ax,k} \\ -r_{ay,k} & r_{ax,k} & 0 \end{bmatrix} := -\tilde{\bold r}_{a,k}

  • Angular velocity changes by angular acceleration times infinitesimal time
  • Ignore change of angular acceleration
  • Acceleration and Magnetometor are subject to change of attitude, which is DCM

Therefore the model matrix G\bold G can be constructed as

Gk=∂g(xk)∂xk=[IIΔt000I00−r~a,kΔt0I+ω~kΔt0−r~m,kΔt00I+ω~kΔt]\bold G_{k} = \dfrac{\partial g(\bold x_k)}{\partial \bold x_k} = \begin{bmatrix} \bold I & \bold I\Delta t & 0 &0 \\ 0 & \bold I & 0 & 0 \\ -\tilde{\bold r}_{a,k}\Delta t & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t & 0 \\ -\tilde{\bold r}_{m,k}\Delta t & 0 & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t \\ \end{bmatrix}

Gk∈R12×12\bold G_{k} \in \R^{12 \times 12}.

The relationship between measurement and state can be characterized as

zk=[I00000I0000I]xk+[vωˉvrˉavrˉm]\bold z_k = \begin{bmatrix} \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold I & 0 \\ 0 & 0 & 0 & \bold I\\ \end{bmatrix} \bold x_k + \begin{bmatrix} \bold v_{\bar \omega} \\ \bold v_{\bar r_a} \\ \bold v_{\bar r_m} \\ \end{bmatrix}

vk\bold v_k is measurement noise. Hk∈R9×12\bold H_k \in \R^{9 \times 12} .

Procedure

Input

x_aposteriori_k[12] x^k∣k\hat{\bold x}_{k|k}

P_aposteriori_k[144] Pk∣k{\bold P}_{k|k}

z[9] zk+1\bold{z}_{k+1}

Output

eulerAngles[3]

Rot_matrix[9]

x_aposteriori[12] x^k+1∣k+1\hat{\bold x}_{k+1|k+1}

P_aposteriori[144] Pk+1∣k+1{\bold P}_{k+1|k+1}

Priori Estimate of State

x^k+1∣k=g(uk+1,x^k∣k)\hat{\bold x}_{k+1|k} = g(\bold u_{k+1}, \hat{\bold x}_{k|k})

Get the angular acceleration from last moment

wak[0] = x_aposteriori_k[3];
wak[1] = x_aposteriori_k[4];
wak[2] = x_aposteriori_k[5];

Construct matrix O[9] which is ω~kT\tilde {\boldsymbol \omega}_k^T

O=[0−ωzωyωz0−ωx−ωyωx0]\bold O = \begin{bmatrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \\ 

O[0] = 0.0F;
O[1] = -x_aposteriori_k[2];
O[2] = x_aposteriori_k[1];
O[3] = x_aposteriori_k[2];
O[4] = 0.0F;
O[5] = -x_aposteriori_k[0];
O[6] = -x_aposteriori_k[1];
O[7] = x_aposteriori_k[0];
O[8] = 0.0F;

Calculate matrix a[9] and b_a[9]. dv0 is the 3×33 \times 3 unit matrix

a=ba=I+OΔt=(I+ω~kΔt)T\bold a = \bold{ba} = \bold I + \bold O \Delta t= (\bold I + \tilde {\boldsymbol \omega}_k \Delta t)^T 

eye(dv0);  
for (i = 0; i < 9; i++) {
  a[i] = (real32_T)dv0[i] + O[i] * dt;
}  
for (i = 0; i < 9; i++) {
  b_a[i] = (real32_T)dv0[i] + O[i] * dt;
}

Remember that

x=[ωω˙rarm]\bold x = \begin{bmatrix} \boldsymbol \omega \\ \dot{\boldsymbol \omega} \\ \bold r_a \\ \bold r_m \end{bmatrix}

Collect ωk\boldsymbol\omega_k

x_n_b[0] = x_aposteriori_k[0];
x_n_b[1] = x_aposteriori_k[1];
x_n_b[2] = x_aposteriori_k[2];

Collect ra\bold r_a

b_x_aposteriori_k[0] = x_aposteriori_k[6];
b_x_aposteriori_k[1] = x_aposteriori_k[7];
b_x_aposteriori_k[2] = x_aposteriori_k[8];

Collect rm\bold r_m

z_n_b[0] = x_aposteriori_k[9];
z_n_b[1] = x_aposteriori_k[10];
z_n_b[2] = x_aposteriori_k[11];

Calculate

ca=OTra=(I+ω~kΔt)rada=OTrm=(I+ω~kΔt)rm\bold{ca} = \bold O^T \bold r_a = (\bold I + \tilde {\boldsymbol \omega}_k \Delta t) \bold r_a \\ \bold{da} = \bold O^T \bold r_m = (\bold I + \tilde {\boldsymbol \omega}_k \Delta t) \bold r_m 

for (i = 0; i < 3; i++) {
	c_a[i] = 0.0F;
	for (i0 = 0; i0 < 3; i0++) {
		c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; // r_a
	}
	d_a[i] = 0.0F;
	for (i0 = 0; i0 < 3; i0++) {
		d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; // r_m
	}
	x_apriori[i] = x_n_b[i] + dt * wak[i]; // add angular velocity times delta t
}

The prior estimation is calculated using linear approximation.

Remember that

Gk+1=[IIΔt000I00−r~a,kΔt0I+ω~kΔt0−r~m,kΔt00I+ω~kΔt]\bold G_{k+1} = \begin{bmatrix} \bold I & \bold I\Delta t & 0 &0 \\ 0 & \bold I & 0 & 0 \\ -\tilde{\bold r}_{a,k}\Delta t & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t & 0 \\ -\tilde{\bold r}_{m,k}\Delta t & 0 & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t \\ \end{bmatrix}

x^k+1∣k=g(uk+1,x^k∣k)≈Gk+1x^k∣k\hat{\bold x}_{k+1|k} = g(\bold u_{k+1}, \hat{\bold x}_{k|k}) \approx \bold G_{k+1} \hat{\bold x}_{k|k}

  • Suppose there is no control vector, i.e. $\bold u_{k+1} =0 $
  • Partition x^k+1∣k\hat{\bold x}_{k+1|k} into four vertical parts, then
    • x^k+1∣k,1=ω+ω˙Δt\hat{\bold x}_{k+1|k, 1} = \boldsymbol \omega + \dot{\boldsymbol \omega} \Delta t
      • This is done in the above code
    • x^k+1∣k,2=ω˙\hat{\bold x}_{k+1|k, 2} = \dot{\boldsymbol \omega}
    • x^k+1∣k,3=ca\hat{\bold x}_{k+1|k, 3} = \bold{ca}
      • Notice that the part −r~a,kΔt-\tilde{\bold r}_{a,k}\Delta t is ignored

    • x^k+1∣k,4=da\hat{\bold x}_{k+1|k, 4} = \bold{da}

      • Notice that the part −r~a,mΔt-\tilde{\bold r}_{a,m}\Delta t is ignored
for (i = 0; i < 3; i++) {
	x_apriori[i + 3] = wak[i];
}
for (i = 0; i < 3; i++) {
	x_apriori[i + 6] = c_a[i];
}
for (i = 0; i < 3; i++) {
	x_apriori[i + 9] = d_a[i];
}

Priori Estimate of State Finished

Priori Estimate of Variance

Pk+1∣k=Gk+1Pk∣kGk+1T+Qk+1\bold P_{k+1|k} = \bold G_{k+1} \bold P_{k|k} \bold G_{k+1}^{T} + \bold Q_{k+1}

Calculate Gk\bold G_k. First Calculate

Alin=[00−r~a,kT−r~a,mTI00000ω~kT0000ω~kT]=[00r~a,kr~a,mI00000O0000O]\bold {Alin} = \begin{bmatrix} 0 & 0 & -\tilde{\bold r}_{a,k}^T & -\tilde{\bold r}_{a,m}^T \\ \bold I & 0 & 0 & 0 \\ 0 & 0 & \tilde{\boldsymbol\omega}_k^T & 0 \\ 0 & 0 & 0 & \tilde{\boldsymbol\omega}_k^T \\ \end{bmatrix} = \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & \tilde{\bold r}_{a,m} \\ \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold O & 0 \\ 0 & 0 & 0 & \bold O \\ \end{bmatrix}

iv0=[0I00]Alin←[00??I0??00??00??]\bold{iv0} = \begin{bmatrix} 0 \\ \bold I \\ 0 \\ 0 \\ \end{bmatrix} \qquad \bold {Alin} \leftarrow \begin{bmatrix} 0 & 0 & ? & ? \\ \bold I & 0 & ? & ? \\ 0 & 0 & ? & ? \\ 0 & 0 & ? & ? \\ \end{bmatrix} 

for (i = 0; i < 12; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
  }
  
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * i) + 3] = 0.0F;
  }
}

Alin←[00r~a,k?I00?00??00??]\bold{Alin} \leftarrow \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & ? \\ \bold I & 0 & 0 & ? \\ 0 & 0 & ? & ? \\ 0 & 0 & ? & ? \\ \end{bmatrix} 

A_lin[6] = 0.0F;
A_lin[7] = x_aposteriori_k[8];
A_lin[8] = -x_aposteriori_k[7];
A_lin[18] = -x_aposteriori_k[8];
A_lin[19] = 0.0F;
A_lin[20] = x_aposteriori_k[6];
A_lin[30] = x_aposteriori_k[7];
A_lin[31] = -x_aposteriori_k[6];
A_lin[32] = 0.0F;

for (i = 0; i < 3; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
  }
}

Alin←[00r~a,k?I00?00O?000?]\bold{Alin} \leftarrow \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & ? \\ \bold I & 0 & 0 & ? \\ 0 & 0 & \bold O & ? \\ 0 & 0 & 0 & ? \\ \end{bmatrix} 

for (i = 0; i < 3; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
  }
}

for (i = 0; i < 3; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
  }
}

Alin←[00r~a,kr~a,mI00000O0000O]\bold{Alin} \leftarrow \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & \tilde{\bold r}_{a,m} \\ \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold O & 0 \\ 0 & 0 & 0 & \bold O \\ \end{bmatrix} 

A_lin[9] = 0.0F;
A_lin[10] = x_aposteriori_k[11];
A_lin[11] = -x_aposteriori_k[10];
A_lin[21] = -x_aposteriori_k[11];
A_lin[22] = 0.0F;
A_lin[23] = x_aposteriori_k[9];
A_lin[33] = x_aposteriori_k[7];
A_lin[34] = -x_aposteriori_k[9];
A_lin[35] = 0.0F;

for (i = 0; i < 3; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
  }
}

for (i = 0; i < 3; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
  }
}

for (i = 0; i < 3; i++) {
  for (i0 = 0; i0 < 3; i0++) {
    A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
  }
}

dv1=[I0000I0000I0000I]bAlin:=Gk+1T=dv1+AlinΔt\bold{dv1} = \begin{bmatrix} \bold I & 0 & 0 & 0 \\ 0 & \bold I & 0 & 0 \\ 0 & 0 & \bold I & 0 \\ 0 & 0 & 0 & \bold I \\ \end{bmatrix} \qquad \bold{bAlin} := \bold G_{k+1}^T = \bold{dv1} + \bold{Alin} \Delta t 

for (i = 0; i < 12; i++) {
  for (i0 = 0; i0 < 12; i0++) {
    b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *dt;   
  }
}

G Calculation Finished

Setup noise coefficient matrix Q\bold Q

  • We view Qk+1=Gk+1QGk+1T\bold Q_{k+1} = \bold G_{k+1} \bold Q \bold G_{k+1}^T

Q=[q10000q20000q30000q4]qi=[qi000qi000qi]\bold Q = \begin{bmatrix} \bold q_1 & 0 & 0 & 0 \\ 0 & \bold q_2 & 0 & 0 \\ 0 & 0 & \bold q_3 & 0 \\ 0 & 0 & 0 & \bold q_4 \\ \end{bmatrix} \qquad \bold q_i = \begin{bmatrix} q_i & 0 & 0 \\ 0 & q_i & 0 \\ 0 & 0 & q_i \\ \end{bmatrix} 

b_q[0] = q[0];
b_q[12] = 0.0F;
b_q[24] = 0.0F;
b_q[36] = 0.0F;
b_q[48] = 0.0F;
b_q[60] = 0.0F;
b_q[72] = 0.0F;
b_q[84] = 0.0F;
b_q[96] = 0.0F;
b_q[108] = 0.0F;
b_q[120] = 0.0F;
b_q[132] = 0.0F;
b_q[1] = 0.0F;
b_q[13] = q[0];
b_q[25] = 0.0F;
b_q[37] = 0.0F;
b_q[49] = 0.0F;
b_q[61] = 0.0F;
b_q[73] = 0.0F;
b_q[85] = 0.0F;
b_q[97] = 0.0F;
b_q[109] = 0.0F;
b_q[121] = 0.0F;
b_q[133] = 0.0F;
b_q[2] = 0.0F;
b_q[14] = 0.0F;
b_q[26] = q[0];
b_q[38] = 0.0F;
b_q[50] = 0.0F;
b_q[62] = 0.0F;
b_q[74] = 0.0F;
b_q[86] = 0.0F;
b_q[98] = 0.0F;
b_q[110] = 0.0F;
b_q[122] = 0.0F;
b_q[134] = 0.0F;
b_q[3] = 0.0F;
b_q[15] = 0.0F;
b_q[27] = 0.0F;
b_q[39] = q[1];
b_q[51] = 0.0F;
b_q[63] = 0.0F;
b_q[75] = 0.0F;
b_q[87] = 0.0F;
b_q[99] = 0.0F;
b_q[111] = 0.0F;
b_q[123] = 0.0F;
b_q[135] = 0.0F;
b_q[4] = 0.0F;
b_q[16] = 0.0F;
b_q[28] = 0.0F;
b_q[40] = 0.0F;
b_q[52] = q[1];
b_q[64] = 0.0F;
b_q[76] = 0.0F;
b_q[88] = 0.0F;
b_q[100] = 0.0F;
b_q[112] = 0.0F;
b_q[124] = 0.0F;
b_q[136] = 0.0F;
b_q[5] = 0.0F;
b_q[17] = 0.0F;
b_q[29] = 0.0F;
b_q[41] = 0.0F;
b_q[53] = 0.0F;
b_q[65] = q[1];
b_q[77] = 0.0F;
b_q[89] = 0.0F;
b_q[101] = 0.0F;
b_q[113] = 0.0F;
b_q[125] = 0.0F;
b_q[137] = 0.0F;
b_q[6] = 0.0F;
b_q[18] = 0.0F;
b_q[30] = 0.0F;
b_q[42] = 0.0F;
b_q[54] = 0.0F;
b_q[66] = 0.0F;
b_q[78] = q[2];
b_q[90] = 0.0F;
b_q[102] = 0.0F;
b_q[114] = 0.0F;
b_q[126] = 0.0F;
b_q[138] = 0.0F;
b_q[7] = 0.0F;
b_q[19] = 0.0F;
b_q[31] = 0.0F;
b_q[43] = 0.0F;
b_q[55] = 0.0F;
b_q[67] = 0.0F;
b_q[79] = 0.0F;
b_q[91] = q[2];
b_q[103] = 0.0F;
b_q[115] = 0.0F;
b_q[127] = 0.0F;
b_q[139] = 0.0F;
b_q[8] = 0.0F;
b_q[20] = 0.0F;
b_q[32] = 0.0F;
b_q[44] = 0.0F;
b_q[56] = 0.0F;
b_q[68] = 0.0F;
b_q[80] = 0.0F;
b_q[92] = 0.0F;
b_q[104] = q[2];
b_q[116] = 0.0F;
b_q[128] = 0.0F;
b_q[140] = 0.0F;
b_q[9] = 0.0F;
b_q[21] = 0.0F;
b_q[33] = 0.0F;
b_q[45] = 0.0F;
b_q[57] = 0.0F;
b_q[69] = 0.0F;
b_q[81] = 0.0F;
b_q[93] = 0.0F;
b_q[105] = 0.0F;
b_q[117] = q[3];
b_q[129] = 0.0F;
b_q[141] = 0.0F;
b_q[10] = 0.0F;
b_q[22] = 0.0F;
b_q[34] = 0.0F;
b_q[46] = 0.0F;
b_q[58] = 0.0F;
b_q[70] = 0.0F;
b_q[82] = 0.0F;
b_q[94] = 0.0F;
b_q[106] = 0.0F;
b_q[118] = 0.0F;
b_q[130] = q[3];
b_q[142] = 0.0F;
b_q[11] = 0.0F;
b_q[23] = 0.0F;
b_q[35] = 0.0F;
b_q[47] = 0.0F;
b_q[59] = 0.0F;
b_q[71] = 0.0F;
b_q[83] = 0.0F;
b_q[95] = 0.0F;
b_q[107] = 0.0F;
b_q[119] = 0.0F;
b_q[131] = 0.0F;
b_q[143] = q[3];

Utilize input Pk∣k\bold P_{k|k} to do the next estimate

Alinj,i←bAlin:,i⋅Pk∣k,j,:=Pk∣k,j,:⋅bAlin:,ior equivalentlyAlin←Pk∣kGk+1TcAlin←QGk+1TdAlin←Gk+1Alin=Gk+1Pk∣kGk+1TeAlin←Gk+1cAlin=Gk+1QGk+1T\bold{Alin}_{j,i} \leftarrow \bold{bAlin}_{:,i} \cdot \bold{P}_{k|k,j,:} = \bold{P}_{k|k,j,:} \cdot \bold{bAlin}_{:,i} \\ \text{or equivalently} \\ \bold{Alin} \leftarrow \bold P_{k|k}\bold G_{k+1}^T \\ \bold{cAlin} \leftarrow \bold Q \bold G_{k+1}^T \\ \bold{dAlin} \leftarrow \bold G_{k+1} \bold{Alin} = \bold G_{k+1} \bold P_{k|k}\bold G_{k+1}^T \\ \bold{eAlin} \leftarrow \bold G_{k+1} \bold{cAlin} = \bold G_{k+1} \bold Q \bold G_{k+1}^T \\ 

for (i = 0; i < 12; i++) {
	for (i0 = 0; i0 < 12; i0++) {
		A_lin[i + 12 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0];
		}

    c_A_lin[i + 12 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
		}
	}

	for (i0 = 0; i0 < 12; i0++) {
		d_A_lin[i + 12 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
		}

		e_A_lin[i + 12 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
		}
	}
}

Finally

Pk+1∣k=dAlin+eAlin=Gk+1Pk∣kGk+1T+Gk+1QGk+1T\bold P_{k+1|k} = \bold{dAlin} + \bold{eAlin} = \bold G_{k+1} \bold P_{k|k}\bold G_{k+1}^T + \bold G_{k+1} \bold Q \bold G_{k+1}^T 

for (i = 0; i < 12; i++) {
	for (i0 = 0; i0 < 12; i0++) {
		P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
	}
}

Priori Estimate of Variance Finished

Calculate Kalman Gain

Kk+1=Pk+1∣kHk+1T(Hk+1Pk+1∣kHk+1T+Rk+1)−1\bold K_{k+1} = \bold P_{k+1|k} \bold H_{k+1}^T(\bold H_{k+1} \bold P_{k+1|k} \bold H_{k+1}^T + \bold R_{k+1})^{-1}

First make sure all sensor values have been updated

if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1))

Here updateVect[0] is for gyroscope, updateVect[1] is for accelerometer, updateVect[2] is for magnetometer.

Remember that Hk+1\bold H_{k+1} is considered as a constant ∈R9×12\in \R^{9 \times 12}

iv1=H=[I00000I0000I]bPk+1∣k=HPk+1∣k\bold {iv1} = \bold H = \begin{bmatrix} \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold I & 0 \\ 0 & 0 & 0 & \bold I\\ \end{bmatrix} \\ \bold{bP}_{k+1|k} = \bold H \bold{P}_{k+1|k} 

if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
	r[1] = 10000.0F;
}
for (i = 0; i < 12; i++) {
	for (i0 = 0; i0 < 9; i0++) {
		b_P_apriori[i + 12 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + 12 * i0];
		}
	}
}

iv2=HT=[I000000I000I]Temp←Pk+1∣kiv2=Pk+1∣kHTfv0←iv1Temp=HPk+1∣kHT\bold {iv2} = \bold H^T = \begin{bmatrix} \bold I & 0 & 0 \\ 0 & 0 & 0 \\ 0 & \bold I & 0 \\ 0 & 0 & \bold I\\ \end{bmatrix} \\ \bold{Temp} \leftarrow \bold{P}_{k+1|k} \bold{iv2} = \bold{P}_{k+1|k}\bold H^T \\ \bold {fv0} \leftarrow \bold{iv1} \bold{Temp} = \bold H \bold{P}_{k+1|k}\bold H^T 

for (i = 0; i < 9; i++) {
	for (i0 = 0; i0 < 12; i0++) {
		K_k[i + 9 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
		}
	}

	for (i0 = 0; i0 < 9; i0++) {
		fv0[i + 9 * i0] = 0.0F;
		for (i1 = 0; i1 < 12; i1++) {
			fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
		}
	}
}

Setup noise coefficient matrix R\bold R

  • We view Rk+1=R\bold R_{k+1} = \bold R.

R=[r1000r2000r3]ri=[ri000ri000ri]fv1=fv0+R=HPk+1∣kHT+R\bold R = \begin{bmatrix} \bold r_1 & 0 & 0 \\ 0 & \bold r_2 & 0 \\ 0 & 0 & \bold r_3 \end{bmatrix} \qquad \bold r_i = \begin{bmatrix} r_i & 0 & 0 \\ 0 & r_i & 0 \\ 0 & 0 & r_i \\ \end{bmatrix} \qquad \bold{fv1} = \bold{fv0} + \bold R = \bold H \bold{P}_{k+1|k}\bold H^T + \bold R 

b_r[0] = r[0];
  b_r[9] = 0.0F;
  b_r[18] = 0.0F;
  b_r[27] = 0.0F;
  b_r[36] = 0.0F;
  b_r[45] = 0.0F;
  b_r[54] = 0.0F;
  b_r[63] = 0.0F;
  b_r[72] = 0.0F;
  b_r[1] = 0.0F;
  b_r[10] = r[0];
  b_r[19] = 0.0F;
  b_r[28] = 0.0F;
  b_r[37] = 0.0F;
  b_r[46] = 0.0F;
  b_r[55] = 0.0F;
  b_r[64] = 0.0F;
  b_r[73] = 0.0F;
  b_r[2] = 0.0F;
  b_r[11] = 0.0F;
  b_r[20] = r[0];
  b_r[29] = 0.0F;
  b_r[38] = 0.0F;
  b_r[47] = 0.0F;
  b_r[56] = 0.0F;
  b_r[65] = 0.0F;
  b_r[74] = 0.0F;
  b_r[3] = 0.0F;
  b_r[12] = 0.0F;
  b_r[21] = 0.0F;
  b_r[30] = r[1];
  b_r[39] = 0.0F;
  b_r[48] = 0.0F;
  b_r[57] = 0.0F;
  b_r[66] = 0.0F;
  b_r[75] = 0.0F;
  b_r[4] = 0.0F;
  b_r[13] = 0.0F;
  b_r[22] = 0.0F;
  b_r[31] = 0.0F;
  b_r[40] = r[1];
  b_r[49] = 0.0F;
  b_r[58] = 0.0F;
  b_r[67] = 0.0F;
  b_r[76] = 0.0F;
  b_r[5] = 0.0F;
  b_r[14] = 0.0F;
  b_r[23] = 0.0F;
  b_r[32] = 0.0F;
  b_r[41] = 0.0F;
  b_r[50] = r[1];
  b_r[59] = 0.0F;
  b_r[68] = 0.0F;
  b_r[77] = 0.0F;
  b_r[6] = 0.0F;
  b_r[15] = 0.0F;
  b_r[24] = 0.0F;
  b_r[33] = 0.0F;
  b_r[42] = 0.0F;
  b_r[51] = 0.0F;
  b_r[60] = r[2];
  b_r[69] = 0.0F;
  b_r[78] = 0.0F;
  b_r[7] = 0.0F;
  b_r[16] = 0.0F;
  b_r[25] = 0.0F;
  b_r[34] = 0.0F;
  b_r[43] = 0.0F;
  b_r[52] = 0.0F;
  b_r[61] = 0.0F;
  b_r[70] = r[2];
  b_r[79] = 0.0F;
  b_r[8] = 0.0F;
  b_r[17] = 0.0F;
  b_r[26] = 0.0F;
  b_r[35] = 0.0F;
  b_r[44] = 0.0F;
  b_r[53] = 0.0F;
  b_r[62] = 0.0F;
  b_r[71] = 0.0F;
  b_r[80] = r[2];
  for (i = 0; i < 9; i++) {
    for (i0 = 0; i0 < 9; i0++) {
      fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
    }
  }

Now perform matrix division to obtain Kalman Gain.

Kk+1(Hk+1Pk+1∣kHk+1T+Rk+1)=Pk+1∣kHk+1TPk+1∣kHk+1T=bPk+1∣kT∈R12×9,Kk+1∈R12×9,(Hk+1Pk+1∣kHk+1T+Rk+1)=fv1∈R9×9\bold K_{k+1}(\bold H_{k+1} \bold P_{k+1|k} \bold H_{k+1}^T + \bold R_{k+1}) = \bold P_{k+1|k} \bold H_{k+1}^T \\ \bold P_{k+1|k} \bold H_{k+1}^T =\bold{bP}_{k+1|k}^T \in \R^{12 \times 9}, \quad \bold K_{k+1} \in \R^{12 \times 9}, \quad (\bold H_{k+1} \bold P_{k+1|k} \bold H_{k+1}^T + \bold R_{k+1})=\bold{fv1} \in \R^{9 \times 9} 

mrdivide(b_P_apriori, fv1, K_k);

Kalman Gain Finished

Posteriori Estimate of State

x^k+1∣k=x^k+1∣k+Kk+1(zk+1−h(x^k+1∣k))\hat{\bold x}_{k+1|k} = \hat{\bold x}_{k+1|k} + \bold K_{k+1}(\bold z_{k+1} - h(\hat{\bold x}_{k+1|k}))

Algorithm
#Estimation theory #Optimal control
PX4 EKF Code Review
http://zzhou.info/2020/10/08/px4-ekf/
Author
Zihao Zhou
Posted on
October 8, 2020
Licensed under