卡尔曼滤波推导笔记

2023-11-03

文章目录

卡尔曼滤波（Kalman Filter）

卡尔曼滤波（Kalman Filter）

Optimal Recursire Data Processing Algorithm（最优化递归数字处理算法）
此篇文章是学习DR_CAN视频个人记录的笔记，若有侵权马上删除

不确定性：

不存在完美的数学模型
系统的扰动不可控，也很难建模
测量的传感器存在误差

第一节递归算法

例子：测量一个硬币的直径

测量结果：

z 1 = 50.1 c m z_1=50.1cm z1=50.1cm
z 2 = 50.4 c m z_2=50.4cm z2=50.4cm
z 3 = 50.2 c m z_3=50.2cm z3=50.2cm
z k z_k zk…（第k次测量结果）

估计真实数据：直接的方法是取平均数

x ^ k = 1 k ( z 1 + z 2 + . . . + z k ) = 1 k ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k = 1 k k − 1 k − 1 ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k = k − 1 k x ^ k − 1 + 1 k z k = x ^ k − 1 − 1 k x ^ k − 1 + 1 k z k x ^ k = x ^ k − 1 + 1 k ( z k − x ^ k − 1 ) \begin{aligned} \hat{x}_{k}&=\frac{1}{k}(z_1+z_2+...+z_k)\\ &=\frac{1}{k}(z_1+z_2+...+z_{k-1}) + \frac{1}{k}z_k\\ &=\frac{1}{k}\frac{k-1}{k-1}(z_1+z_2+...+z_{k-1}) + \frac{1}{k}z_k\\ &=\frac{k-1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k\\ &=\hat{x}_{k-1}-\frac{1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k\\ \hat{x}_{k}&=\hat{x}_{k-1}+\frac{1}{k}(z_k-\hat{x}_{k-1}) \end{aligned} x^kx^k=k1(z1+z2+...+zk)=k1(z1+z2+...+zk−1)+k1zk=k1k−1k−1(z1+z2+...+zk−1)+k1zk=kk−1x^k−1+k1zk=x^k−1−k1x^k−1+k1zk=x^k−1+k1(zk−x^k−1)
随着k增加，测量结果不再重要；k比较小的时候，测量结果 z k z_k zk作用比较大。

上面的式子用 K k K_k Kk来代替系数，公式变成 x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_{k}=\hat{x}_{k-1}+K_k(z_k-\hat{x}_{k-1}) x^k=x^k−1+Kk(zk−x^k−1)

表达为：当前估计值=上一次估计值+系数×（当前测量值-上一次估计值）

K k K_k Kk：Kalman Gain （卡尔曼增益）

估计误差（Error Estimate）： e E S T e_{EST} eEST

测量误差（Error Measurement）： e M E A e_{MEA} eMEA
K k = e E S T k − 1 e E S T k − 1 + e M E A k K_k=\frac{{e_{EST}}_{k-1}}{{e_{EST}}_{k-1}+{e_{MEA}}_{k}} Kk=eESTk−1+eMEAkeESTk−1
k时刻，估计误差远大于测量误差时，卡尔曼增益趋近于1，更相信测量结果；
估计误差远小于测量误差时，卡尔曼增益趋近于0，更相信上一次估计结果。

Step 1：计算卡尔曼增益 K k K_k Kk;

Step 2：计算此次估计值 x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_{k}=\hat{x}_{k-1}+K_k(z_k-\hat{x}_{k-1}) x^k=x^k−1+Kk(zk−x^k−1)

Step 3：更新 e E S T k = ( 1 − K k ) e E S T k − 1 {e_{EST}}_k=(1-K_k){e_{EST}}_{k-1} eESTk=(1−Kk)eESTk−1

k k k	z k z_k zk	e M E A k {e_{MEA}}_{k} eMEAk	x ^ k \hat{x}_{k} x^k	K k K_k Kk	e E S T k {e_{EST}}_k eESTk
0			40（随机估计）		5
1	51	3	46.875	0.625	1.875
2	48	3	47.308	0.3846	1.154
3	…

第二节数学基础

数据融合、协方差矩阵、状态空间方程、观测器

Data Fuslen、Covariance Matrix、State Space、Observation

数据融合（Data Fuslen）

正态分布（高斯分布）： σ σ σ为标准差

z 1 = 30 g , σ 1 = 2 g z_1=30g, σ_1=2g z1=30g,σ1=2g

z 2 = 32 g , σ 2 = 4 g z_2=32g, σ_2=4g z2=32g,σ2=4g

估计真实值 z ^ = ? \hat{z}=? z^=?， z ^ = z 1 + K ( z 2 − z 1 ) \hat{z}=z_1+K(z_2-z_1) z^=z1+K(z2−z1)，求k使得 z ^ \hat{z} z^的方差最小
σ z 2 = V a r ( z 1 + K ( z 2 − z 1 ) ) = V a r ( z 1 − K z 1 + K z 2 ) = V a r ( ( 1 − K ) z 1 + K z 2 ) （ z 1 与 z 2 相互独立） = V a r ( ( 1 − K ) z 1 ) + V a r ( K z 2 ) = ( 1 − K ) 2 V a r ( z 1 ) + K 2 V a r ( z 2 ) = ( 1 − K ) 2 σ 1 2 + K 2 σ 2 2 \begin{aligned} σ_z^2&=Var(z_1+K(z_2-z_1))\\ &=Var(z_1-Kz_1+Kz_2)\\ &=Var((1-K)z_1+Kz_2)（z_1与z_2相互独立）\\ &=Var((1-K)z_1)+Var(Kz_2)\\ &=(1-K)^2Var(z_1)+K^2Var(z_2)\\ &=(1-K)^2σ_1^2+K^2σ_2^2 \end{aligned} σz2=Var(z1+K(z2−z1))=Var(z1−Kz1+Kz2)=Var((1−K)z1+Kz2)（z1与z2相互独立）=Var((1−K)z1)+Var(Kz2)=(1−K)2Var(z1)+K2Var(z2)=(1−K)2σ12+K2σ22
求导 d σ ^ 2 d K = 0 \frac{d{\hat{σ}^2}}{dK}=0 dKdσ^2=0：
− 2 ( 1 − K ) σ 1 2 + 2 K σ 2 2 = 0 − σ 1 2 + K σ 1 2 + K σ 2 2 = 0 K ( σ 1 2 + σ 2 2 ) = σ 1 2 K = σ 1 2 σ 1 2 + σ 2 2 = 2 2 2 2 + 4 2 = 4 4 + 16 = 0.2 \begin{aligned} -2(1-K)σ_1^2+2Kσ_2^2&=0\\ -σ_1^2+Kσ_1^2+Kσ_2^2&=0\\ K(σ_1^2+σ_2^2)&=σ_1^2\\ K&=\frac{σ_1^2}{σ_1^2+σ_2^2}=\frac{2^2}{2^2+4^2}=\frac{4}{4+16}=0.2 \end{aligned} −2(1−K)σ12+2Kσ22−σ12+Kσ12+Kσ22K(σ12+σ22)K=0=0=σ12=σ12+σ22σ12=22+4222=4+164=0.2
代回上面的式子中： z ^ = z 1 + K ( z 2 − z 1 ) = 30 + 0.2 ( 32 − 30 ) = 30.4 \hat{z}=z_1+K(z_2-z_1)=30+0.2(32-30)=30.4 z^=z1+K(z2−z1)=30+0.2(32−30)=30.4

这个过程就叫做数据融合。

协方差矩阵（Covariance Matrix）

体现变量之间的联动关系

例子：

球员	身高（x）	体重（y）	年龄（z）
瓦尔迪	179	74	33
奥巴梅扬	187	80	31
萨拉赫	175	71	28
平均值	180.3	75	30.7
方差	24.89	14	4.22

协方差：
σ x σ y = 1 3 ( ( 179 − 180.3 ) ( 74 − 75 ) + ( 187 − 180.3 ) ( 80 − 75 ) + ( 175 − 180.3 ) ( 71 − 75 ) ) = 18.7 = σ y σ x σ x σ z = 4.4 = σ z σ x σ y σ z = 3.3 = σ z σ y \begin{aligned} σ_xσ_y&=\frac{1}{3}((179-180.3)(74-75)+(187-180.3)(80-75)+(175-180.3)(71-75))=18.7=σ_yσ_x\\ σ_xσ_z&=4.4=σ_zσ_x\\ σ_yσ_z&=3.3=σ_zσ_y\\ \end{aligned} σxσyσxσzσyσz=31((179−180.3)(74−75)+(187−180.3)(80−75)+(175−180.3)(71−75))=18.7=σyσx=4.4=σzσx=3.3=σzσy
协方差矩阵：
P = [ σ x 2 σ x σ y σ x σ z σ y σ x σ y 2 σ y σ z σ z σ x σ z σ y σ z 2 ] = [ 24.8 18.7 4.4 18.7 14 3.3 4.4 3.3 4.22 ] P= \left[ \begin{matrix} σ_x^2 & σ_xσ_y & σ_xσ_z\\ σ_yσ_x & σ_y^2 & σ_yσ_z \\ σ_zσ_x & σ_zσ_y & σ_z^2 \end{matrix} \right]= \left[ \begin{matrix} 24.8 & 18.7 & 4.4\\ 18.7 & 14 & 3.3 \\ 4.4 & 3.3 & 4.22 \end{matrix} \right] P= σx2σyσxσzσxσxσyσy2σzσyσxσzσyσzσz2 = 24.818.74.418.7143.34.43.34.22
过渡矩阵（实际上是求值减去平均值）：
a = [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] − 1 3 [ 1 1 1 1 1 1 1 1 1 ] [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] a= \left[ \begin{matrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{matrix} \right] -\frac{1}{3} \left[ \begin{matrix} 1 & 1 & 1\\ 1 & 1 & 1 \\ 1 & 1 & 1 \end{matrix} \right] \left[ \begin{matrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{matrix} \right] a= x1x2x3y1y2y3z1z2z3 −31 111111111 x1x2x3y1y2y3z1z2z3

P = 1 3 a T a P=\frac{1}{3}a^Ta P=31aTa

状态空间表达（State Space Representation）

例子：

一个弹簧系统：物体质量为m，向右施加力是F，向右的方向x，弹簧的胡可系数是k，阻尼系数是B。

动态方程表达式： m x ¨ + B x ˙ + k x = F = u m\ddot{x}+B\dot{x}+kx=F=u mx¨+Bx˙+kx=F=u :Input，这里x的点表示导数。

状态变量： x 1 = x , x 2 = x ˙ x_1=x, x_2=\dot{x} x1=x,x2=x˙, 则
x ˙ 1 = x 2 x ˙ 2 = x ¨ = 1 m u − B m x ˙ − k m x = 1 m u − B m x 2 − k m x 1 \begin{aligned} \dot{x}_1&=x_2\\ \dot{x}_2&=\ddot{x}=\frac{1}{m}u-\frac{B}{m}\dot{x}-\frac{k}{m}x\\ &=\frac{1}{m}u-\frac{B}{m}x_2-\frac{k}{m}x_1 \end{aligned} x˙1x˙2=x2=x¨=m1u−mBx˙−mkx=m1u−mBx2−mkx1
测量（Measurement）：
z 1 = x = x 1 ：位置 z 2 = x ˙ = x 2 ：速度 \begin{aligned} z_1&=x=x_1：位置\\ z_2&=\dot{x}=x_2：速度\\ \end{aligned} z1z2=x=x1：位置=x˙=x2：速度
转成矩阵：

[ x ˙ 1 x ˙ 2 ] = [ 0 1 − k m − B m ] [ x 1 x 2 ] + [ 0 1 m ] u \begin{aligned}\left[\begin{matrix}\dot{x}_1\\ \dot{x}_2 \\\end{matrix}\right] &=\left[\begin{matrix}0 & 1\\-\frac{k}{m} & -\frac{B}{m}\\\end{matrix}\right] \left[\begin{matrix}x_1\\x_2\\\end{matrix}\right]+\left[\begin{matrix}0\\\frac{1}{m}\\\end{matrix}\right]u\\ \end{aligned} [x˙1x˙2]=[0−mk1−mB][x1x2]+[0m1]u

[ z 1 z 2 ] = [ 1 0 0 1 ] [ x 1 x 2 ] \begin{aligned} \left[\begin{matrix}z_1\\z_2\\\end{matrix}\right]&=\left[\begin{matrix}1 & 0\\0 & 1\\\end{matrix}\right] \left[\begin{matrix}x_1\\x_2\\\end{matrix}\right] \end{aligned} [z1z2]=[1001][x1x2]

归纳出来就是：
X ˙ ( t ) = A X ( t ) + B u ( t ) Z ( t ) = H X ( t ) \begin{aligned} \dot{X}(t)&=AX(t)+Bu(t)\\ Z(t)&=HX(t) \end{aligned} X˙(t)Z(t)=AX(t)+Bu(t)=HX(t)
离散表达：
X k = A X k + B u k + w k − 1 ，（ w 是过程噪音，表达不确定性） Z k = H X k + v k ，（ v 是测量噪音） \begin{aligned} X_k&=AX_k+Bu_k+w_{k-1}，（w是过程噪音，表达不确定性）\\ Z_k&=HX_k+v_k，（v是测量噪音） \end{aligned} XkZk=AXk+Buk+wk−1，（w是过程噪音，表达不确定性）=HXk+vk，（v是测量噪音）
模型不准确、测量不准确，在二者噪音存在的情况下，确定 X ^ = ? \hat{X}=? X^=?

第三节卡尔曼增益推导

自然界中噪声一般符合正态分布，则P(w)~(0, Q) ， Q为协方差矩阵。 Q = E ( w w T ) Q=E(ww^T) Q=E(wwT)，下面证明：

假设有 [ x 1 x 2 ] \left[\begin{matrix}x_1\\x_2\\\end{matrix}\right] [x1x2]与 [ w 1 w 2 ] \left[\begin{matrix}w_1\\w_2\\\end{matrix}\right] [w1w2]，则有：
E [ [ w 1 w 2 ] [ w 1 w 2 ] ] = [ E w 1 2 E w 1 w 2 E w 2 w 1 E w 2 2 ] \begin{aligned} E[\left[\begin{matrix}w_1\\w_2\\\end{matrix}\right]\left[\begin{matrix}w_1&w_2\\\end{matrix}\right]] &=\left[ \begin{matrix} Ew_1^2 & Ew_1w_2\\ Ew_2w_1 & Ew_2^2\\ \end{matrix} \right]\\ \end{aligned} E[[w1w2][w1w2]]=[Ew12Ew2w1Ew1w2Ew22]
∵ V a r ( x ) = E ( x 2 ) − E 2 ( x ) E ( w ) = 0 \because Var(x)=E(x^2)-E^2(x)\\ E(w)=0 ∵Var(x)=E(x2)−E2(x)E(w)=0

∴ E ( w w T ) = [ σ w 1 2 σ w 1 σ w 2 σ w 2 σ w 1 σ w 2 2 ] = Q \therefore E(ww^T)=\left[ \begin{matrix} σ_{w1}^2 & σ_{w1}σ_{w2}\\ σ_{w2}σ_{w1} & σ_{w2}^2\\ \end{matrix} \right]=Q ∴E(wwT)=[σw12σw2σw1σw1σw2σw22]=Q

P(v)~(0, R)， E ( v v T ) = R E(vv^T)=R E(vvT)=R

建模的过程中，噪声无法建模：
X k = A X k + B u k Z k = H X k \begin{aligned} X_k&=AX_k+Bu_k\\ Z_k&=HX_k \end{aligned} XkZk=AXk+Buk=HXk
此为我们实际能够掌握的地方。实际上这个式子不完整， X k X_k Xk只能是估计值，因此要加上 ^ \hat{} ^，用负号代表它为先验估计：
X ^ k − = A X ^ k + B u k ，（算出来的结果） Z k = H X k → X ^ k m e a = H − Z k ，（测出来的结果） \begin{aligned} \hat{X}_k^-&=A\hat{X}_k+Bu_k，（算出来的结果）\\ Z_k&=HX_k \rightarrow \hat{X}_{k_{mea}}=H^-Z_k，（测出来的结果） \end{aligned} X^k−Zk=AX^k+Buk，（算出来的结果）=HXk→X^kmea=H−Zk，（测出来的结果）
不管是算出来的结果还是测出来的结果，都不具备对噪声的影响。卡尔曼滤波就是通过两个不太准确的结果得出相对准确的结果。后验 X ^ k \hat{X}_k X^k
X ^ k = X ^ k − + G ( H − Z k − X ^ k − ) \hat{X}_k=\hat{X}_k^-+G(H^-Z_k-\hat{X}_k^-) X^k=X^k−+G(H−Zk−X^k−)
令 G = K k H G=K_kH G=KkH
X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat{X}_k=\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-) X^k=X^k−+Kk(Zk−HX^k−)
目标：寻找 K k K_k Kk ，使得 x k ^ → x k ( 真实值 ) \hat{x_k}\rightarrow x_k(真实值) xk^→xk(真实值)

e k = x k − x ^ k e_k=x_k-\hat{x}_k ek=xk−x^k ，P(e)~(0, P)

P = E [ e e T ] = [ σ e 1 2 σ e 1 σ e 2 σ e 2 σ e 1 σ e 2 2 ] P=E[ee^T]=\left[\begin{matrix}σe_1^2&σe_1σe_2\\σe_2σe_1&σe_2^2\\\end{matrix}\right] P=E[eeT]=[σe12σe2σe1σe1σe2σe22] ，要使误差方差最小，我们希望选取合适的K，使得P的迹最小：

t r ( P ) = σ e 1 2 + σ e 2 2 tr(P)=σe_1^2+σe_2^2 tr(P)=σe12+σe22

P = E [ e e T ] = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] ( ∵ x ^ k = x ^ k − + K k ( z k − H x ^ k − ) ) ( ∵ z k = H x k + v k ) = E [ [ ( I − K k H ) e k − − K k v k ] [ ( I − K k H ) e k − − K k v k ] T ] ( ∵ ( A B ) T = B T A T ; ( A + B ) T = A T + B T ) = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − v k T K k T − K k v k e k − T ( I − K k H ) T + K k v k v k T K k T ] \begin{aligned} P&=E[ee^T]\\ &=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T](\because \hat{x}_k=\hat{x}_k^-+K_k(z_k-H\hat{x}_k^-)) (\because z_k=Hx_k+v_k)\\ &=E[[(I-K_kH)e_k^--K_kv_k][(I-K_kH)e_k^--K_kv_k]^T](\because (AB)^T=B^TA^T;(A+B)^T=A^T+B^T)\\ &=E[(I-K_kH)e_k^-{e_k^-}^T(I-K_kH)^T-(I-K_kH)e_k^-v_k^TK_k^T-K_kv_k{e_k^-}^T(I-K_kH)^T+K_kv_kv_k^TK_k^T]\\ \end{aligned} P=E[eeT]=E[(xk−x^k)(xk−x^k)T](∵x^k=x^k−+Kk(zk−Hx^k−))(∵zk=Hxk+vk)=E[[(I−KkH)ek−−Kkvk][(I−KkH)ek−−Kkvk]T](∵(AB)T=BTAT;(A+B)T=AT+BT)=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−vkTKkT−Kkvkek−T(I−KkH)T+KkvkvkTKkT]

∵ e k 与 v k 相互独立， E ( e k − ) = E ( v k T ) = 0 \because e_k与v_k相互独立，E(e_k^-)=E(v_k^T)=0 ∵ek与vk相互独立，E(ek−)=E(vkT)=0

∴ P = ( I − K k H ) E ( e k − e k − T ) ( I − K k H ) T + K k E ( v k v k T ) K k T , ( P k − = E ( e k − e k − T ) ) = ( P k − − K k H P k − ) ( I T − H T K k T ) + K k R K k T P k = P k − − K k H P k − − P k − H T K k T + K k H P k − H T K k T + K k R K k T t r ( P k ) = t r ( P k − ) − 2 t r ( K k H p k − ) + t r ( K k H P k − H T K k T ) + t r ( K k R K k T ) \begin{aligned} \therefore P&=(I-K_kH)E(e_k^-{e_k^-}^T)(I-K_kH)^T+K_kE(v_kv_k^T)K_k^T, (P_k^-=E(e_k^-{e_k^-}^T))\\ &=(P_k^--K_kHP_k^-)(I^T-H^TK_k^T)+K_kRK_k^T\\ P_k&=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T\\ tr(P_k)&=tr(P_k^-)-2tr(K_kHp_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kRK_k^T)\\ \end{aligned} ∴PPktr(Pk)=(I−KkH)E(ek−ek−T)(I−KkH)T+KkE(vkvkT)KkT,(Pk−=E(ek−ek−T))=(Pk−−KkHPk−)(IT−HTKkT)+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT=tr(Pk−)−2tr(KkHpk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)
要使 t r ( P k ) tr(P_k) tr(Pk) 有最小值：
d t r ( P k ) d K k = 0 , ∵ d t r ( A B ) d A = B T , d t r ( A B A T ) d A = 2 A B d t r ( P k ) d K k = 0 − 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R = 0 − P k − H T + K k ( H P k − H T + R ) = 0 K k = P k − H T H P k − H T + R \begin{aligned} \frac{dtr(P_k)}{dK_k}&=0, \because \frac{dtr(AB)}{dA}=B^T,\frac{dtr(ABA^T)}{dA}=2AB\\ \frac{dtr(P_k)}{dK_k}&=0-2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR=0\\ -P_k^-H^T+K_k(HP_k^-H^T+R)&=0\\ K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} \end{aligned} dKkdtr(Pk)dKkdtr(Pk)−Pk−HT+Kk(HPk−HT+R)Kk=HPk−HT+RPk−HT=0,∵dAdtr(AB)=BT,dAdtr(ABAT)=2AB=0−2(HPk−)T+2KkHPk−HT+2KkR=0=0
这就把卡尔曼增益表达出来了。

第四节误差协方差矩阵推导

X k = A X k − 1 + B u k − 1 + w k − 1 ，（ w ∼ P ( 0 , Q ) 是过程噪音，表达不确定性） Z k = H X k + v k ，（ v ∼ P ( 0 , R ) 是测量噪音） \begin{aligned} X_k&=AX_{k-1}+Bu_{k-1}+w_{k-1}，（w \sim P(0, Q)是过程噪音，表达不确定性）\\ Z_k&=HX_k+v_k，（v \sim P(0, R)是测量噪音） \end{aligned} XkZk=AXk−1+Buk−1+wk−1，（w∼P(0,Q)是过程噪音，表达不确定性）=HXk+vk，（v∼P(0,R)是测量噪音）

先验估计： X ^ k − = A X ^ k − 1 + B u k − 1 \hat{X}_k^-=A\hat{X}_{k-1}+Bu_{k-1} X^k−=AX^k−1+Buk−1

后验估计： X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat{X}_k=\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-) X^k=X^k−+Kk(Zk−HX^k−)

卡尔曼增益： K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPk−HT+RPk−HT

求 P k − P_k^- Pk−:
e k − = X k − X ^ k − = A X k − 1 + B u k − 1 + w k − 1 − A X ^ k − 1 − B u k − 1 = A ( X k − 1 − X ^ k − 1 ) + w k − 1 = A e k − 1 + w k − 1 P k − = E [ e k − e k − T ] = E [ ( A e k − 1 + w k − 1 ) ( e k − 1 T A T + w k − 1 T ) ] = E [ A e k − 1 e k − 1 T A T ] + E [ A e k − 1 w k − 1 T ] + E [ w k − 1 e k − 1 T A T ] + E [ w k − 1 w k − 1 T ] = A E [ e k − 1 e k − 1 T ] A T + E [ w k − 1 w k − 1 T ] = A P k − 1 A T + Q \begin{aligned} e_k^-&=X_k-\hat{X}_k^-\\ &=AX_{k-1}+Bu_{k-1}+w_{k-1}-A\hat{X}_{k-1}-Bu_{k-1}\\ &=A(X_{k-1}-\hat{X}_{k-1})+w_{k-1}\\ &=Ae_{k-1}+w_{k-1}\\ P_k^-&=E[e_k^-{e_k^-}^T]\\ &=E[(Ae_{k-1}+w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)]\\ &=E[Ae_{k-1}e_{k-1}^TA^T]+E[Ae_{k-1}w_{k-1}^T]+E[w_{k-1}e_{k-1}^TA^T]+E[w_{k-1}w_{k-1}^T]\\ &=AE[e_{k-1}e_{k-1}^T]A^T+E[w_{k-1}w_{k-1}^T]\\ &=AP_{k-1}A^T+Q \end{aligned} ek−Pk−=Xk−X^k−=AXk−1+Buk−1+wk−1−AX^k−1−Buk−1=A(Xk−1−X^k−1)+wk−1=Aek−1+wk−1=E[ek−ek−T]=E[(Aek−1+wk−1)(ek−1TAT+wk−1T)]=E[Aek−1ek−1TAT]+E[Aek−1wk−1T]+E[wk−1ek−1TAT]+E[wk−1wk−1T]=AE[ek−1ek−1T]AT+E[wk−1wk−1T]=APk−1AT+Q

	预测		校正
先验	X ^ k − = A X ^ k − 1 + B u k − 1 \hat{X}_k^-=A\hat{X}_{k-1}+Bu_{k-1} X^k−=AX^k−1+Buk−1	卡尔曼增益	K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPk−HT+RPk−HT
先验误差协方差	P k − = A P k − 1 A T + Q P_k^-=AP_{k-1}A^T+Q Pk−=APk−1AT+Q	后验估计	X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat{X}_k=\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-) X^k=X^k−+Kk(Zk−HX^k−)
		更新误差协方差	P k = ( I − K k H ) P k − P_k=(I-K_kH)P_k^- Pk=(I−KkH)Pk−

P k = P k − − K k H P k − − P k − H T K k T + K k H P k − H T K k T + K k R K k T P_k=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T Pk=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT

第六节扩展卡尔曼滤波器

解决非线性系统的卡尔曼滤波

非线性系统：
X k = f ( X k − 1 , u k − 1 , w k − 1 ) ，（ w ∼ P ( 0 , Q ) 是过程噪音，表达不确定性） Z k = h ( X k , v k ) ，（ v ∼ P ( 0 , R ) 是测量噪音） f , h 是非线性的 \begin{aligned} X_k&=f(X_{k-1}, u_{k-1}, w_{k-1})，（w \sim P(0, Q)是过程噪音，表达不确定性）\\ Z_k&=h(X_k, v_k)，（v \sim P(0, R)是测量噪音）\\ &f,h是非线性的 \end{aligned} XkZk=f(Xk−1,uk−1,wk−1)，（w∼P(0,Q)是过程噪音，表达不确定性）=h(Xk,vk)，（v∼P(0,R)是测量噪音）f,h是非线性的
正态分布的随机变量通过非线性系统后就不是正态的了。

线性化（linearization）：用泰勒级数（Taylor Series）对k-1进行展开

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

算法