零基础学习卡尔曼滤波器

netouch 2024-03-26 发布于北京

展开全文

提示：

公式比较多，但是都是很基础的公式，基本高中数学就足够了。非常建议大家泡杯茶、吹个头、点开一个舒缓的音乐、调整一下椅子、抽出纸笔，拿出一晚上时间，耐心慢慢看，祝君好运！

参考

【卡尔曼滤波器】,真的是大佬讲课！！牛逼就完事了。（主要参考）
图解卡尔曼滤波(Kalman Filter)

1. 递归算法_Recursive Processing（体验）

假设我们我们测量一个硬币的直径，每次测量不准确，但是我们进过多次测量。例如每次测量 50、 51、 49，然后我们可以平均值，假设经过了k次测量，有如下。

x k = 1 k ( z 1 + z 2 + . . . + z k ) x_k=\frac{1}{k}(z_1+z_2+...+z_k) xk=k1(z1+z2+...+zk)

x k = 1 k ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k x_k=\frac{1}{k}(z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k xk=k1(z1+z2+...+zk−1)+k1zk

x k = 1 k k − 1 k − 1 ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k x_k=\frac{1}{k}\frac{k-1}{k-1} (z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k xk=k1k−1k−1(z1+z2+...+zk−1)+k1zk

x k = k − 1 k ( 1 k − 1 ( z 1 + z 2 + . . . + z k − 1 ) ) + 1 k z k x_k=\frac{k-1}{k}(\frac{1}{k-1} (z_1+z_2+...+z_{k-1}))+\frac{1}{k}z_k xk=kk−1(k−11(z1+z2+...+zk−1))+k1zk

x k = k − 1 k ( x k − 1 ) + 1 k z k x_k=\frac{k-1}{k}(x_{k-1})+\frac{1}{k}z_k xk=kk−1(xk−1)+k1zk

x k = x k − 1 − 1 k ( x k − 1 ) + 1 k z k x_k=x_{k-1}-\frac{1}{k}(x_{k-1})+\frac{1}{k}z_k xk=xk−1−k1(xk−1)+k1zk

x k = x k − 1 + 1 k ( z k − x k − 1 ) x_k=x_{k-1}+\frac{1}{k}(z_k-x_{k-1}) xk=xk−1+k1(zk−xk−1)

可以看出，随着测量次数k的增加，当前测量值对 x k x_k xk预测的影响越来越小。

这里将 1 k \frac{1}{k} k1用 K k K_k Kk代替，可以得到下面的公式：

x k = x k − 1 + K k ( z k − x k − 1 ) x_k=x_{k-1}+K_k(z_k-x_{k-1}) xk=xk−1+Kk(zk−xk−1)

上面公式使用语言描述：当前估计值=上一次估计值+系数×（当前测量值-上一次的估计值），其中这个系数 K k K_k Kk就是卡尔曼增益。这样可以进行连续的递归的估计。

这里介绍一下 K k K_k Kk是怎么进行计算的（不用过于纠结这里，只是一个感性的认识，后面会详细推导），首先定义一下估计误差 E e s t E_{est} Eest和测量误差 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

分析可得：当估计误差 E e s t E_{est} Eest很大时，得到的结果偏向于当先测量值。
分析可得：当测量误差 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
计算当前估计值： x k = x k − 1 + K k ( z k − x k − 1 ) x_k=x_{k-1}+K_k(z_k-x_{k-1}) xk=xk−1+Kk(zk−xk−1)
更新估计误差： 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，（这里没有更新测量误差，因为我们认为测量误差是恒定的）该公式之后讲。

利用上面的简单的卡尔曼滤波器计算下面问题：假设硬币实际长度位50mm，估计长度为40mm，估计误差为5mm，第一次测量值为51mm，测量误差为3mm，有多次测量值可以得到下面的表格。

次数k	测量值Z_k	测量误差E_mea（恒定）	估计值x_k	估计误差E_est
0			40	5
1	51	3
2	48	3
3	52	3
4	49	3

按照上面的简单的卡尔曼滤波器进行计算，可以一直向下进行计算。可以使用程序或者excel进行计算。使用C++完成上面任务，具体代码如下(懒了没写)：

2. 数学基础：数据融合、协方差矩阵、状态空间方程

2.1 数据融合

首先什么叫数据融合，举一个例子来说明。假设我用两个秤去秤一个苹果的质量，第一个 Z 1 Z_1 Z1秤精度高一点测到为30g标准差为2g，第二个 Z 2 Z_2 Z2秤的精度低一些侧到为32g标准差为4g。假设这两个秤的测量结果服从正态分布。现在我们需要利用这两个秤的数据得到一个更加准确的苹果质量的数据，这么做呢？

标准差：标准差是方差的算术平方根。标准差能反映一个数据集的离散程度。平均数相同的两组数据，标准差未必相同。
标准差 σ = s q r t ( ( ( x 1 − x ) 2 + ( x 2 − x ) 2 + . . . . . . ( x n − x ) 2 ) / n ) σ=sqrt(((x1-x)^2 +(x2-x)^2 +......(xn-x)^2)/n ) σ=sqrt(((x1−x)2+(x2−x)2+......(xn−x)2)/n)其中 x x x为一组数（n个数据）的算术平均值（数学期望）。

正态分布：（Normal distribution），也称“常态分布”，又名高斯分布（Gaussian distribution）。若随机变量X服从一个数学期望为μ、方差为 σ 2 σ^2 σ2的正态分布，记为 N ( μ ， σ 2 ) N(μ，σ^2) N(μ，σ2)。其概率密度函数为正态分布的期望值μ决定了其位置，其标准差σ决定了分布的幅度。

Z = Z 1 + K ( Z 2 − Z 1 ) Z=Z_1+K(Z_2-Z_1) Z=Z1+K(Z2−Z1)

我们需要计算出苹果更加准确的质量，意味着我们希望得到的结果的标准差（方差）要尽量的的小。那么怎么计算得到的结果的方差呢？看如下计算过程：

问题：求 K 使得方差 V a r ( Z ) 最小。 V a r ( Z ) = V a r ( Z 1 + K ( Z 2 − Z 1 ) ) = V a r ( Z 1 + K Z 2 − K Z 1 ) = V a r ( ( 1 − K ) Z 1 + K 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 ) 上式中 V a r ( Z 1 ) 和 V a r ( Z 2 ) 就是两个测量的方差，题目中的 2 g 和 4 g 问题：求K使得方差Var(Z)最小。\\ Var(Z) = Var(Z_1+K(Z_2-Z_1)) \\ =Var(Z_1+KZ_2-KZ_1) \\ = Var((1-K)Z_1+KZ_2) \\ 两个秤的测量肯定是相互独立的，所以得到下面：\\ =Var((1-K)Z_1)+Var(KZ_2) \\ =(1-K)^2Var(Z_1)+K^2Var(Z_2) \\ 上式中Var(Z_1)和Var(Z_2)就是两个测量的方差，题目中的2g 和 4g \\ 问题：求K使得方差Var(Z)最小。Var(Z)=Var(Z1+K(Z2−Z1))=Var(Z1+KZ2−KZ1)=Var((1−K)Z1+KZ2)两个秤的测量肯定是相互独立的，所以得到下面：=Var((1−K)Z1)+Var(KZ2)=(1−K)2Var(Z1)+K2Var(Z2)上式中Var(Z1)和Var(Z2)就是两个测量的方差，题目中的2g和4g

既然是要求 K 使得方差 V a r ( Z ) 最小，那就直接对最后的公式对 K 求导等于 0 就完事了。 d V a r ( Z ) d K = 0 − 2 ( 1 − K ) V a r ( Z 1 ) + 2 K V a r ( Z 2 ) = 0 − V a r ( Z 1 ) + K V a r ( Z 1 ) + K V a r ( Z 2 ) = 0 K V a r ( Z 1 ) + K V a r ( Z 2 ) = V a r ( Z 1 ) K ( V a r ( Z 1 ) + V a r ( Z 2 ) ) = V a r ( Z 1 ) K = V a r ( Z 1 ) V a r ( Z 1 ) + V a r ( Z 2 ) 完成，这里求出 K 的计算公式。既然是要求K使得方差Var(Z)最小，\\ 那就直接对最后的公式对K求导等于0就完事了。\\ \frac{d_{Var(Z)}}{d_K}=0 \\ -2(1-K)Var(Z_1)+2KVar(Z_2)=0\\ -Var(Z_1)+KVar(Z_1)+KVar(Z_2)=0 \\ KVar(Z_1)+KVar(Z_2)=Var(Z_1) \\ K(Var(Z_1)+Var(Z_2))=Var(Z_1) \\ K = \frac{Var(Z_1)}{Var(Z_1)+Var(Z_2)} \\ 完成，这里求出K的计算公式。既然是要求K使得方差Var(Z)最小，那就直接对最后的公式对K求导等于0就完事了。dKdVar(Z)=0−2(1−K)Var(Z1)+2KVar(Z2)=0−Var(Z1)+KVar(Z1)+KVar(Z2)=0KVar(Z1)+KVar(Z2)=Var(Z1)K(Var(Z1)+Var(Z2))=Var(Z1)K=Var(Z1)+Var(Z2)Var(Z1)完成，这里求出K的计算公式。

上式带入具体数字即可计算结果。 K = 2 2 2 2 + 4 2 = 4 4 + 16 = 1 5 = 0.2 带入上式得到： Z = Z 1 + 0.2 ( Z 2 − Z 1 ) = 30 + 0.2 ( 32 − 30 ) = 30.4 ( 最优解 ) 计算新的估计值的标准差： V a r ( Z ) = ( 1 − K ) 2 V a r ( Z 1 ) + K 2 V a r ( Z 2 ) = ( 1 − 0.2 ) 2 ∗ 2 2 + 0. 2 2 ∗ 4 2 = 3.2 开根号得到标准差为： σ Z = 1.79 ，比两个测量值的标准差都小。上式带入具体数字即可计算结果。\\ K = \frac{2^2}{2^2+4^2}=\frac{4}{4+16}=\frac{1}{5}=0.2\\ 带入上式得到：\\ Z=Z_1+0.2(Z_2-Z_1)=30+0.2(32-30)=30.4(最优解) \\ 计算新的估计值的标准差：\\ Var(Z)=(1-K)^2Var(Z_1)+K^2Var(Z_2)=(1-0.2)^2*2^2+0.2^2*4^2=3.2\\ 开根号得到标准差为：\sigma_Z =1.79 ，比两个测量值的标准差都小。上式带入具体数字即可计算结果。K=22+4222=4+164=51=0.2带入上式得到：Z=Z1+0.2(Z2−Z1)=30+0.2(32−30)=30.4(最优解)计算新的估计值的标准差：Var(Z)=(1−K)2Var(Z1)+K2Var(Z2)=(1−0.2)2∗22+0.22∗42=3.2开根号得到标准差为：σZ=1.79，比两个测量值的标准差都小。

以上就是数据融合的过程和理念，大家可以好好体会一下。

2.2 协方差矩阵

协方差矩阵:将方差、协方差在一个矩阵中表示出来–>体现变量之间的联系

举一个例子就很好明白协方差矩阵是什么东西了，如下：

球员	身高x	体重y	年龄z
A	179	74	33
B	187	80	31
C	175	71	28

首先分别计算这三名球员的身高、体重以及年龄的平均值。可得平均身高=180.3、平均体重=75、平均年龄=30.7。

然后计算方差：

σ x 2 = 1 3 ( ( 179 − 180.3 ) 2 + ( 187 − 180.3 ) 2 + ( 175 − 180.3 ) 2 ) = 24.89 同理可以计算其他的方差 , ( 方差能反映一个数据集的离散程度。 ) σ y 2 = 14 σ z 2 = 4.22 \sigma _x^2=\frac{1}{3}((179-180.3)^2+(187-180.3)^2+(175-180.3)^2)=24.89 \\ 同理可以计算其他的方差,(方差能反映一个数据集的离散程度。)\\ \sigma _y^2=14 \\ \sigma _z^2=4.22 σx2=31((179−180.3)2+(187−180.3)2+(175−180.3)2)=24.89同理可以计算其他的方差,(方差能反映一个数据集的离散程度。)σy2=14σz2=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 σ y σ z = 3.3 \sigma _x \sigma _y = \frac{1}{3}((179-180.3)(74-75)+(187-180.3)(80-75)\\ +(175-180.3)(71-75))=18.7= \sigma _y \sigma _x \\ 其他也可以同理进行计算：\\ \sigma _x \sigma _z = 4.4\\ \sigma _y \sigma _z = 3.3\\ σxσy=31((179−180.3)(74−75)+(187−180.3)(80−75)+(175−180.3)(71−75))=18.7=σyσx其他也可以同理进行计算：σxσz=4.4σyσz=3.3

于是可以得到协方差矩阵：

P = [ σ x 2 σ x σ y σ x σ z σ y σ x σ y 2 σ y σ z σ z σ x σ z σ y σ z 2 ] = [ 24.89 18.7 4.4 18.7 14 3.3 4.4 3.3 4.22 ] P= \begin{bmatrix} \sigma _x^2 & \sigma _x \sigma _y & \sigma _x \sigma _z\\ \sigma _y \sigma _x & \sigma _y^2 & \sigma _y \sigma _z\\ \sigma _z \sigma _x & \sigma _z \sigma _y &\sigma _z^2 \end{bmatrix}\\ =\begin{bmatrix} 24.89 & 18.7 & 4.4\\ 18.7 & 14 & 3.3\\ 4.4 & 3.3 &4.22 \end{bmatrix} P= σx2σyσxσzσxσxσyσy2σzσyσxσzσyσzσz2 = 24.8918.74.418.7143.34.43.34.22

协方差矩阵为什么可以体现变量之间的联系？协方差矩阵中方差是体现一种数据的分布的离散程度，而协方差才是体现两个数据之间的系的部分。哪协方差是怎么体现处两个数据之间的联系的呢？仔细看上面计算协方差的公式：

σ x σ y = 1 3 ( ( 179 − 180.3 ) ( 74 − 75 ) + ( 187 − 180.3 ) ( 80 − 75 ) + ( 175 − 180.3 ) ( 71 − 75 ) ) = 18.7 = σ y σ x \sigma _x \sigma _y = \frac{1}{3}((179-180.3)(74-75)+(187-180.3)(80-75)\\ +(175-180.3)(71-75))=18.7= \sigma _y \sigma _x σxσy=31((179−180.3)(74−75)+(187−180.3)(80−75)+(175−180.3)(71−75))=18.7=σyσx

可以看出协方差是有两个部分相乘然后累加得到的：

如果这两个部分同时为负数（即两个数据都小于平均值）则为正，累加得到的数会更大。
如果这两个部分同时为正数（即两个数据都大于平均值）则为正，累加得到的数会更大。
如果这两个部分一个为正一个为负数（即两个数据一个大于平均值一个小于平均值）则为负，累加得到的数会更小。

这就表明：

如果两个这两个数为正相关（我大你也大，我小你也小）则得到的协方差会很大（正值）。
如果两个这两个数为负相关（我大你也小，我小你也大）则得到的协方差会很小（负值）。

所以协方差可以体现变量之间的联系：

协方差大（正值）–>则为正相关
协方差小（负值）–>则为负相关
协方差接近于0–>则为不相关

这里说明一种矩阵计算协方差矩阵的方法，在编程中方便计算（DR_CAN）：

过度矩阵 a 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 ] P = 1 3 a T a 过度矩阵a \\ a==\begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ x_3 & y_3 & z_3 \end{bmatrix} -\frac{1}{3} \begin{bmatrix} 1 & 1 & 1\\ 1 & 1 & 1\\ 1 & 1 & 1 \end{bmatrix} \begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ x_3 & y_3 & z_3 \end{bmatrix} \\ P=\frac{1}{3}a^Ta 过度矩阵aa== x1x2x3y1y2y3z1z2z3 −31 111111111 x1x2x3y1y2y3z1z2z3 P=31aTa

2.3 状态空间方程

这个和我们的要学的内容slam关系不大，知道下面的就行。

预测方程： x k = A x k − 1 + B u k + w k − 1 观测方程： z k = H x k + v k 预测方程：x_k=Ax_{k-1}+Bu_{k}+w_{k-1} \\ 观测方程：z_k=Hx_k+v_k 预测方程：xk=Axk−1+Buk+wk−1观测方程：zk=Hxk+vk

其中 w k − 1 w_{k-1} wk−1为过程噪声，服从 p ( w ) ∼ ( 0 , Q ) p(w)\sim (0,Q) p(w)∼(0,Q)期望为0，协方差矩阵为Q的（多维）正态分布。
并且， v k v_{k} vk为测量噪声，服从 p ( v ) ∼ ( 0 , R ) p(v)\sim (0,R) p(v)∼(0,R)期望为0，协方差矩阵为R的（多维）正态分布。

下面介绍一下如何计算协方差Q和R（两个同理，举例计算一个）：

首先直接给出结果： Q = E [ w w T ] ( 测量噪声 : R = E [ v v T ] ) 假设 w k 有两个量为 [ w 1 , w 2 ] T Q = E [ [ w 1 w 2 ] [ w 1 w 2 ] ] Q = E [ w 1 2 w 1 w 2 w 2 w 1 w 2 2 ] 首先直接给出结果：Q=E[ww^T]\\ (测量噪声:R=E[vv^T])\\ 假设w_k有两个量为[w_1,w_2]^T\\ Q= E\begin{bmatrix} \begin{bmatrix} w_1 \\ w_2 \end{bmatrix} &\begin{bmatrix} w_1 & w_2 \end{bmatrix} \end{bmatrix} \\ Q = E\begin{bmatrix} w_1^2 & w_1w_2\\ w_2w_1 &w_2^2 \end{bmatrix} 首先直接给出结果：Q=E[wwT](测量噪声:R=E[vvT])假设wk有两个量为[w1,w2]TQ=E[[w1w2][w1w2]]Q=E[w12w2w1w1w2w22]

大家应该都记得高中 ( 大学 ) 线性代数中学过一个重要的公式（反正我不记得）： x 的方差 = x 平方的期望 − x 期望的平方 , 数学形式如下 V a r ( x ) = E ( x 2 ) − E 2 ( x ) 对于噪声，其数学期望为 0 ，所以上面公式可写成如下： V a r ( x ) = E ( x 2 ) 大家应该都记得高中(大学)线性代数中学过一个重要的公式（反正我不记得）：\\ x的方差=x平方的期望-x期望的平方,数学形式如下\\ Var(x)=E(x^2)-E^2(x) \\ 对于噪声，其数学期望为0，所以上面公式可写成如下：\\ Var(x)=E(x^2) \\ 大家应该都记得高中(大学)线性代数中学过一个重要的公式（反正我不记得）：x的方差=x平方的期望−x期望的平方,数学形式如下Var(x)=E(x2)−E2(x)对于噪声，其数学期望为0，所以上面公式可写成如下：Var(x)=E(x2)

然后将这个公式带入上面矩阵中，可得： Q = [ σ w 1 2 σ w 1 σ w 2 σ w 2 σ w 1 σ w 2 2 ] 这就是协方差矩阵！！！然后将这个公式带入上面矩阵中，可得：\\ Q = \begin{bmatrix} \sigma _{w_1^2} & \sigma _{w_1}\sigma _{w_2}\\ \sigma _{w_2}\sigma _{w_1} &\sigma _{w_2^2} \end{bmatrix}\\ 这就是协方差矩阵！！！然后将这个公式带入上面矩阵中，可得：Q=[σw12σw2σw1σw1σw2σw22]这就是协方差矩阵！！！

3. 卡尔曼增益超详细数学推导

（字太多了，不能发布，剩下的在下一章。）提示：一大波公式即将来临

下面这些不用看了，少年，去下一个任务点吧！！
求导可得如下： 0 − 2 ( H p k − ) T + 2 K k H p k − H T + 2 K k R = 0 − p k − T H T + K k H p k − H T + K k R = 0 − p k − T H T + K k ( H p k − H T + R ) = 0 协方差矩阵的转置是其本身，所以有如下： − p k − H T + K k ( H p k − H T + R ) = 0 K k ( H p k − H T + R ) = p k − H T 最终得到 K k 的表达式： K k = p k − H T H p k − H T + R − − − − − − ( 3 ) 这个就是大名鼎鼎的卡尔曼增益！！！！求导可得如下：\\ 0-2(Hp_k^-)^T+2K_kHp_k^-H^T+2K_kR=0\\ -p_k^{-T}H^T+K_kHp_k^-H^T+K_kR=0\\ -p_k^{-T}H^T+K_k(Hp_k^-H^T+R)=0\\ 协方差矩阵的转置是其本身，所以有如下：\\ -p_k^{-}H^T+K_k(Hp_k^-H^T+R)=0\\ K_k(Hp_k^-H^T+R)=p_k^{-}H^T\\ 最终得到K_k的表达式：\\ K_k=\frac{p_k^{-}H^T}{Hp_k^-H^T+R}------(3)\\ 这个就是大名鼎鼎的卡尔曼增益！！！！求导可得如下：0−2(Hpk−)T+2KkHpk−HT+2KkR=0−pk−THT+KkHpk−HT+KkR=0−pk−THT+Kk(Hpk−HT+R)=0协方差矩阵的转置是其本身，所以有如下：−pk−HT+Kk(Hpk−HT+R)=0Kk(Hpk−HT+R)=pk−HT最终得到Kk的表达式：Kk=Hpk−HT+Rpk−HT−−−−−−(3)这个就是大名鼎鼎的卡尔曼增益！！！！

4. 误差协方差矩阵的推导

这里回忆一下好像之前我们推导过这个 p k − 1 往上翻我们找到方程（ 7 ） p k = p k − − p k − H T K k T − K k H p k − + K k H p k − H T K k T + K k R K k T 我们进行变换： p k = p k − − p k − H T K k T − K k H p k − + K k ( H p k − H T + R ) K k T 然后将卡尔曼增益 K k 带入第三部分可得如下： p k = p k − − p k − H T K k T − K k H p k − + p k − H T H p k − H T + R ( H p k − H T + R ) K k T p k = p k − − p k − H T K k T − K k H p k − + p k − H T K k T 第二部分和最后一部分相同，抵消掉，得到如下： p k = p k − − K k H p k − p k = ( I − K k H ) p k − − − − − （ 8 ）这就是完成了误差协方差的更新。这里回忆一下好像之前我们推导过这个p_{k-1}\\ 往上翻我们找到方程（7）\\ p_k=p_k^--p_k^-H^TK_k^T-K_kHp_k^-+K_kHp_k^-H^TK_k^T+K_kRK_k^T\\ 我们进行变换：\\ p_k=p_k^--p_k^-H^TK_k^T-K_kHp_k^-+K_k(Hp_k^-H^T+R)K_k^T\\ 然后将卡尔曼增益K_k带入第三部分可得如下：\\ p_k=p_k^--p_k^-H^TK_k^T-K_kHp_k^-+\frac{p_k^{-}H^T}{Hp_k^-H^T+R}(Hp_k^-H^T+R)K_k^T\\ p_k=p_k^--p_k^-H^TK_k^T-K_kHp_k^-+p_k^{-}H^TK_k^T\\ 第二部分和最后一部分相同，抵消掉，得到如下：\\ p_k=p_k^--K_kHp_k^-\\ p_k=(I-K_kH)p_k^-----（8）\\ 这就是完成了误差协方差的更新。这里回忆一下好像之前我们推导过这个pk−1往上翻我们找到方程（7）pk=pk−−pk−HTKkT−KkHpk−+KkHpk−HTKkT+KkRKkT我们进行变换：pk=pk−−pk−HTKkT−KkHpk−+Kk(Hpk−HT+R)KkT然后将卡尔曼增益Kk带入第三部分可得如下：pk=pk−−pk−HTKkT−KkHpk−+Hpk−HT+Rpk−HT(Hpk−HT+R)KkTpk=pk−−pk−HTKkT−KkHpk−+pk−HTKkT第二部分和最后一部分相同，抵消掉，得到如下：pk=pk−−KkHpk−pk=(I−KkH)pk−−−−−（8）这就是完成了误差协方差的更新。

5. 总结

预测：先验估计： x ^ k − = A x ^ k − 1 + B u k 先验误差协方差： p k − = A p k − 1 A T + Q 预测：\\ 先验估计：\hat{x}^-_k=A\hat{x}_{k-1}+Bu_{k}\\ 先验误差协方差：p^-_k=Ap_{k-1}A^T+Q\\ 预测：先验估计：x^k−=Ax^k−1+Buk先验误差协方差：pk−=Apk−1AT+Q

矫正：卡尔曼增益： K k = p k − H T H p k − H T + R 后验估计： x ^ k = x ^ k − + K k ( z k − H x ^ k − ) 更新误差协方差： p k = ( I − K k H ) p k − 矫正：\\ 卡尔曼增益：K_k=\frac{p_k^{-}H^T}{Hp_k^-H^T+R}\\ 后验估计：\hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)\\ 更新误差协方差：p_k=(I-K_kH)p_k^-\\ 矫正：卡尔曼增益：Kk=Hpk−HT+Rpk−HT后验估计：x^k=x^k−+Kk(zk−Hx^k−)更新误差协方差：pk=(I−KkH)pk−