北京建设工程交易协会网站/百度网站的优化方案
文章目录
- 一、滤波的基本概念
- 1、传统数字滤波器
- 2、现代控制中的状态观测器
- 3、最优估计的含义
- 4、温度估计的例子
- 1.问题描述
- 2.分析
- 二、递推最小二乘
课程链接：https://www.bilibili.com/video/BV11K411J7gp/?p=1
参考书目：《捷联惯导算法与组合导航原理》-严恭敏、《卡尔曼滤波与组合导航原理》-秦永元、《Kalman滤波理论及其在导航系统中的应用》-付梦印、《惯性仪器测试与数据分析》-严恭敏
先修课程：矩阵论、数理统计（随机过程）、自动控制原理（现代控制）、数字信号处理（我都没学过，不影响看）
一、滤波的基本概念
1、传统数字滤波器
主要是基于频带来设计，单输入单输出系统。认为数字滤波器处理的是确定性信号，有用的信号在确定的频带内，要去除的噪声在频带之外；噪声在频带内，就没法处理
两种方法：①模拟转数字②直接数字滤波器
MATLAB工具箱fdatool，可以很方便的设置数字滤波器：
2、现代控制中的状态观测器
- 如果参数都知道，上下两部分就只差了一个观测器矩阵
E，类似与Kalman滤波中的K矩阵 - 现代控制中处理的是确定性信号，可以用函数来描述，而估计器处理的是带误差的信号
3、最优估计的含义
每一个分量的二阶矩都达到最小：
E [ ( X ~ k ( 1 ) ) 2 ] + E [ ( X ~ k ( 2 ) ) 2 ] + ⋯ + E [ ( X ~ k ( n ) ) 2 ] = min \mathrm{E}\left[\left(\tilde{X}{k}{(1)}\right){2}\right]+\mathrm{E}\left[\left(\tilde{X}{k}{(2)}\right){2}\right]+\cdots+\mathrm{E}\left[\left(\tilde{X}_{k}{(n)}\right){2}\right]=\min E[(X~k(1))2]+E[(X~k(2))2]+⋯+E[(X~k(n))2]=min
即： E [ X ~ k T X ~ k ] = min \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k}^{\mathrm{T}} \tilde{\boldsymbol{X}}_{k}\right]=\min E[X~kTX~k]=min：
E [ X ~ k X ~ k T ] = [ E [ ( X k ( 1 ) ) 2 ] E [ X k ( 1 ) X k ( 2 ) ] ⋯ E [ X k ( 1 ) X k ( n ) ] E [ X ~ k ( 2 ) X ~ k ( 1 ) ] E [ ( X ~ k ( 2 ) ) 2 ] ⋯ E [ X ~ k ( 2 ) X ~ k ( n ) ] ⋮ ⋮ ⋱ ⋮ E [ X ~ k ( n ) X ~ k ( 1 ) ] E [ X ~ k ( n ) X ~ k ( 2 ) ] ⋯ E [ ( X ~ k ( n ) ) 2 ] ] \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right]=\left[\begin{array}{cccc}\mathrm{E}\left[\left(X_{k}^{(1)}\right)^{2}\right] & \mathrm{E}\left[X_{k}^{(1)} X_{k}^{(2)}\right] & \cdots & \mathrm{E}\left[X_{k}^{(1)} X_{k}^{(n)}\right] \\ \mathrm{E}\left[\tilde{X}_{k}^{(2)} \tilde{X}_{k}^{(1)}\right] & \mathrm{E}\left[\left(\tilde{X}_{k}^{(2)}\right)^{2}\right] & \cdots & \mathrm{E}\left[\tilde{X}_{k}^{(2)} \tilde{X}_{k}^{(n)}\right] \\ \vdots & \vdots & \ddots & \vdots \\ \mathrm{E}\left[\tilde{X}_{k}^{(n)} \tilde{X}_{k}^{(1)}\right] & \mathrm{E}\left[\tilde{X}_{k}^{(n)} \tilde{X}_{k}^{(2)}\right] & \cdots & \mathrm{E}\left[\left(\tilde{X}_{k}^{(n)}\right)^{2}\right]\end{array}\right] E[X~kX~kT]=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡E[(Xk(1))2]E[X~k(2)X~k(1)]⋮E[X~k(n)X~k(1)]E[Xk(1)Xk(2)]E[(X~k(2))2]⋮E[X~k(n)X~k(2)]⋯⋯⋱⋯E[Xk(1)Xk(n)]E[X~k(2)X~k(n)]⋮E[(X~k(n))2]⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
只需要关注对角线上的元素，即求迹：
tr ( P k ) = tr ( E [ X ~ k X ~ k T ] ) = min \operatorname{tr}\left(\boldsymbol{P}_{k}\right)=\operatorname{tr}\left(\mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right]\right)=\min tr(Pk)=tr(E[X~kX~kT])=min
4、温度估计的例子
1.问题描述
- 某房间内温度受随机干扰影响——不恒定、波动
- 每小时用温度计测量一次温度——离散观测点
- 试对该房间温度作最佳估计——建模
- 干扰： W ∼ N ( 0 , 0. 4 ∧ 2 ) W \sim N \left(0,0 .{4^{\wedge} 2}\right) W∼N(0,0.4∧2)——实际参数波动
- 温度计误差 ： V ∼ N ( 0 , 0. 3 ∧ 2 ) V \sim N\left(0,0.3^{\wedge} 2\right) V∼N(0,0.3∧2)——观测值噪声
2.分析
-
假设知道上一时刻温度 25℃，可以知道这一小时的温度受到干扰影响；根据高斯分布，67%概率在干扰的范围内 25±0.4℃，
-
如果温度计读数是 25.2℃，根据高斯分布，67%概率温度在 25.2±0.3℃
-
现在有两方面的信息，一个是惯性保持下来的 25±0.4℃，一个是量测得到的 25.2±0.3℃。如何得出此房间的温度？
-
按照最朴素的想法：两个值加权平均。0.4 代表误差大一些，0.3 代表误差小一些，量测信息更准确，占的权重更大。
-
假设两个值不相关，按概率论方法，可得到：
![[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jUo9Ucjq-1685885499739)(卡尔曼滤波与组合导航原理（一）卡尔曼滤波方程的推导.assets/1685795429896.png)]](https://img-blog.csdnimg.cn/6e5ebb0da4624b5587a872b86ffe452a.png)
可以看出，一个值的权重取决于另一个值的误差，另一个值误差很小，这个权重就小
-
再往下一时刻，算法也同样
![[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-LVEaFehN-1685885499740)(卡尔曼滤波与组合导航原理（一）卡尔曼滤波方程的推导.assets/1685795612188.png)]](https://img-blog.csdnimg.cn/d85b1a27ae3c44ffbe362dc61bf2aa7d.png)
-
从噪声（方差）的角度看，这其实很类似电路，噪声往里加就像串联电路，量测信息和预测信息的结合就像是并联电路
-
如果干扰 W=0，则上一时刻温度是多少，下一时刻温度还是多少；房间是常温，温度计量测带误差，就相当于对常量进行估计，用递推最小二乘法。
-
如果量测噪声 V=0，温度计很准，量出来多少就是多少，变成确定性系统，不用估计了。
二、递推最小二乘
最小二乘量测模型：
Z k = H k X + V k E [ V k ] = 0 E [ V k V j T ] = R k δ k j \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}+\boldsymbol{V}_{k} \quad \mathrm{E}\left[\boldsymbol{V}_{k}\right]=\mathbf{0} \quad \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\boldsymbol{R}_{k} \delta_{k j} Zk=HkX+VkE[Vk]=0E[VkVjT]=Rkδkj
状态方差也有，就是常值，状态转移矩阵就是单位阵，所以一般省略
每一个观测时刻 K K K ，都有一组量测方程 Z k = H k X + V k \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}+\boldsymbol{V}_{k} Zk=HkX+Vk 。一般认为设计矩阵 H k \boldsymbol{H}_{k} Hk 的列数 n 是确定的（代表参数个数不变），行数 m m m 是不确定的（代表观测值个数可变）。可以把很多观测时刻的数据都列拼接在一起：
Z ‾ i = [ Z 1 Z 2 ⋮ Z i ] , H ‾ i = [ H 1 H 2 ⋮ H i ] , V ‾ i = [ V 1 V 2 ⋮ V i ] \overline{\boldsymbol{Z}}_{i}=\left[\begin{array}{c}\boldsymbol{Z}_{1} \\ \boldsymbol{Z}_{2} \\ \vdots \\ \boldsymbol{Z}_{i}\end{array}\right], \quad \overline{\boldsymbol{H}}_{i}=\left[\begin{array}{c}\boldsymbol{H}_{1} \\ \boldsymbol{H}_{2} \\ \vdots \\ \boldsymbol{H}_{i}\end{array}\right], \quad \overline{\boldsymbol{V}}_{i}=\left[\begin{array}{c}\boldsymbol{V}_{1} \\ \boldsymbol{V}_{2} \\ \vdots \\ \boldsymbol{V}_{i}\end{array}\right] Zi=⎣⎢⎢⎢⎡Z1Z2⋮Zi⎦⎥⎥⎥⎤,Hi=⎣⎢⎢⎢⎡H1H2⋮Hi⎦⎥⎥⎥⎤,Vi=⎣⎢⎢⎢⎡V1V2⋮Vi⎦⎥⎥⎥⎤
当 i = k − 1 i=k-1 i=k−1 时刻，对此方程做加权最小二乘估计：
X ^ k − 1 = ( H ‾ k − 1 T R ‾ k − 1 − 1 H ‾ k − 1 ) − 1 H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 = P k − 1 H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 \hat{\boldsymbol{X}}_{k-1} =\left(\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{H}}_{k-1}\right)^{-1} \overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1} =\boldsymbol{P}_{k-1} \overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1} X^k−1=(Hk−1TRk−1−1Hk−1)−1Hk−1TRk−1−1Zk−1=Pk−1Hk−1TRk−1−1Zk−1
上式利用到了前面所有时刻的观测值。同理当 i = k i=k i=k 时的最小二乘估计：
X ^ k = ( H ‾ k T R ‾ k − 1 H ‾ k ) − 1 H ‾ l T R ‾ k − 1 Z ‾ k = P k ( [ H ‾ k − 1 T H k T ] [ R ‾ k − 1 − 1 0 0 R k − 1 ] [ Z ‾ k − 1 $ Z k ] ) = P k ( H ‾ Σ − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 + H k T R 1 − 1 Z k ) \begin{array}{l}\hat{\boldsymbol{X}}_{k}=\left(\overline{\boldsymbol{H}}_{k}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k}^{-1} \overline{\boldsymbol{H}}_{k}\right)^{-1} \overline{\boldsymbol{H}}_{l}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k}^{-1} \overline{\boldsymbol{Z}}_{k} \\ =\boldsymbol{P}_{k}\left(\left[\begin{array}{ll}\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} & \boldsymbol{H}_{k}^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}\overline{\boldsymbol{R}}_{k-1}^{-1} \mathbf{0} & \\ \mathbf{0} & \boldsymbol{R}_{k}^{-1}\end{array}\right]\left[\begin{array}{c}\overline{\boldsymbol{Z}}_{k-1} \\ \$ \boldsymbol{Z}_{k}\end{array}\right]\right) \\ =\boldsymbol{P}_{k}\left(\overline{\boldsymbol{H}}_{\mathrm{\Sigma}-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{1}^{-1} \boldsymbol{Z}_{k}\right) \\\end{array} X^k=(HkTRk−1Hk)−1HlTRk−1Zk=Pk([Hk−1THkT][Rk−1−100Rk−1][Zk−1$Zk])=Pk(HΣ−1TRk−1−1Zk−1+HkTR1−1Zk)
递推目标函数：知道前一时刻的估计值 X ^ k − 1 \hat{\boldsymbol{X}}_{k-1} X^k−1、 P k − 1 \boldsymbol{P}_{k-1} Pk−1 矩阵，和当前时刻测量得来的信息 H k , R k , Z k \boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k} Hk,Rk,Zk ，求得当前时刻的 H k , R k , Z k \boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k} Hk,Rk,Zk
( H k , R k , Z k ) = f ( X ^ k − 1 , P k − 1 , H k , R k , Z k ) \left(\boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k}\right)=f\left(\hat{\boldsymbol{X}}_{k-1}, \boldsymbol{P}_{k-1}, \boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k}\right) (Hk,Rk,Zk)=f(X^k−1,Pk−1,Hk,Rk,Zk)
方差阵递推（协方差传播定律）：
P k = ( H ‾ k T R ‾ k − 1 H ‾ k ) − 1 = ( [ H ‾ k − 1 T H k T ] [ R ‾ k − 1 − 1 0 0 R k − 1 ] [ H ‾ k − 1 H k ] ) − 1 = ( H ‾ k − 1 T R ‾ k − 1 − 1 H ‾ k − 1 + H k T R k − 1 H k ) − 1 = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 \begin{aligned} \boldsymbol{P}_{k} & =\left(\overline{\boldsymbol{H}}_{k}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k}^{-1} \overline{\boldsymbol{H}}_{k}\right)^{-1} \\ & =\left(\left[\begin{array}{ll}\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} & \boldsymbol{H}_{k}^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}\overline{\boldsymbol{R}}_{k-1}^{-1} & \mathbf{0} \\ \mathbf{0} & \boldsymbol{R}_{k}^{-1}\end{array}\right]\left[\begin{array}{c}\overline{\boldsymbol{H}}_{k-1} \\ \boldsymbol{H}_{k}\end{array}\right]\right)^{-1} \\ & =\left(\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{H}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \\ & =\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}\end{aligned} Pk=(HkTRk−1Hk)−1=([Hk−1THkT][Rk−1−100Rk−1][Hk−1Hk])−1=(Hk−1TRk−1−1Hk−1+HkTRk−1Hk)−1=(Pk−1−1+HkTRk−1Hk)−1
上式求逆太多，可以写为下面逆的形式：
P k − 1 = P k − 1 − 1 + H k T R k − 1 H k \boldsymbol{P}_{k}^{-1}=\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} Pk−1=Pk−1−1+HkTRk−1Hk
状态估计递推：
X ^ k = P k ( H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 + H k T R k − 1 Z k ) = P k ( P k − 1 − 1 P k − 1 H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 + H k T R k − 1 Z k ) = P k ( P k − 1 − 1 X ^ k − 1 + H k T R k − 1 Z k ) = P k [ ( P k − 1 − H k T R k − 1 H k ) X ^ k − 1 + H k T R k − 1 Z k ] = X ^ k − 1 + P k H k T R k − 1 ( Z k − H k X ^ k − 1 ) \begin{aligned} \hat{\boldsymbol{X}}_{k} & =\boldsymbol{P}_{k}\left(\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right)=\boldsymbol{P}_{k}\left(\boldsymbol{P}_{k-1}^{-1} \boldsymbol{P}_{k-1} \overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right) \\ & =\boldsymbol{P}_{k}\left(\boldsymbol{P}_{k-1}^{-1} \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right)=\boldsymbol{P}_{k}\left[\left(\boldsymbol{P}_{k}^{-1}-\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right) \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right] \\ & =\hat{\boldsymbol{X}}_{k-1}+\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) \end{aligned} X^k=Pk(Hk−1TRk−1−1Zk−1+HkTRk−1Zk)=Pk(Pk−1−1Pk−1Hk−1TRk−1−1Zk−1+HkTRk−1Zk)=Pk(Pk−1−1X^k−1+HkTRk−1Zk)=Pk[(Pk−1−HkTRk−1Hk)X^k−1+HkTRk−1Zk]=X^k−1+PkHkTRk−1(Zk−HkX^k−1)
上式已经是递推最小二乘了，但为了更接近Kalman滤波，还可以继续向下推导。由于求逆特别多，引入矩阵求逆引理：
对非奇异矩阵 A \boldsymbol{A} A 及子矩阵 A 11 , A 22 \boldsymbol{A}_{11}, \boldsymbol{A}_{22} A11,A22, 若 A = [ A 11 A 12 A 21 A 22 ] \boldsymbol{A}=\left[\begin{array}{ll}\boldsymbol{A}_{11} & \boldsymbol{A}_{12} \\ \boldsymbol{A}_{21} & \boldsymbol{A}_{22}\end{array}\right] A=[A11A21A12A22] ，把 A \boldsymbol{A} A LU分解成求逆简单的三角阵，再求逆:
A = [ I n 0 A 21 A 11 − 1 I m ] [ A 11 A 12 0 A 22 − A 21 A 11 − 1 A 12 ] \boldsymbol{A}=\left[\begin{array}{cc}\boldsymbol{I}_{n} & \mathbf{0} \\ \boldsymbol{A}_{21} \boldsymbol{A}_{11}^{-1} & \boldsymbol{I}_{m}\end{array}\right]\left[\begin{array}{cc}\boldsymbol{A}_{11} & \boldsymbol{A}_{12} \\ \mathbf{0} & \boldsymbol{A}_{22}-\boldsymbol{A}_{21} \boldsymbol{A}_{11}^{-1} \boldsymbol{A}_{12}\end{array}\right] A=[InA21A11−10Im][A110A12A22−A21A11−1A12][ I n 0 A 21 A 11 − 1 I m ] − 1 = [ I n 0 − A 21 A 11 − 1 I m ] \left[\begin{array}{cc}\boldsymbol{I}_{n} & \mathbf{0} \\ \boldsymbol{A}_{21} \boldsymbol{A}_{11}^{-1} & \boldsymbol{I}_{m}\end{array}\right]^{-1}=\left[\begin{array}{cc}\boldsymbol{I}_{n} & \mathbf{0} \\ -\boldsymbol{A}_{21} \boldsymbol{A}_{11}^{-1} & \boldsymbol{I}_{m}\end{array}\right] [InA21A11−10Im]−1=[In−A21A11−10Im]
[ A 11 A 12 0 A 22 − A 21 A 11 − 1 A 12 ] − 1 = [ A 11 − 1 − A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 0 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 ] \left[\begin{array}{cc}A_{11} & A_{12} \\ 0 & A_{22}-A_{21} A_{11}^{-1} A_{12}\end{array}\right]^{-1}=\left[\begin{array}{cc}A_{11}^{-1} & -A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} \\ \mathbf{0} & \left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1}\end{array}\right] [A110A12A22−A21A11−1A12]−1=[A11−10−A11−1A12(A22−A21A11−1A12)−1(A22−A21A11−1A12)−1]
求逆后的两式相乘得：
A − 1 = [ A 11 − 1 + A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 A 21 A 11 − 1 − A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 − ( A 22 − A 21 A 11 − 1 A 12 ) − 1 A 21 A 11 − 1 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 ] = [ ( A 11 − A 12 A 22 − 1 A 21 ) − 1 − ( A 11 − A 12 A 22 − 1 A 21 ) − 1 A 12 A 22 − 1 − A 22 − 1 A 21 ( A 11 − A 12 A 22 − 1 A 21 ) − 1 A 22 − 1 + A 22 − 1 A 21 ( A 11 − A 12 A 22 − 1 A 21 ) − 1 A 12 A 22 − 1 ] \begin{aligned}A^{-1} & =\left[\begin{array}{cc}A_{11}^{-1}+A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} A_{21} A_{11}^{-1} & -A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} \\ -\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} A_{21} A_{11}^{-1} & \left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1}\end{array}\right] \\ & =\left[\begin{array}{cc}\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1} & -\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1} A_{12} A_{22}^{-1} \\ -A_{22}^{-1} A_{21}\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1} & A_{22}^{-1}+A_{22}^{-1} A_{21}\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1} A_{12} A_{22}^{-1}\end{array}\right]\end{aligned} A−1=[A11−1+A11−1A12(A22−A21A11−1A12)−1A21A11−1−(A22−A21A11−1A12)−1A21A11−1−A11−1A12(A22−A21A11−1A12)−1(A22−A21A11−1A12)−1]=[(A11−A12A22−1A21)−1−A22−1A21(A11−A12A22−1A21)−1−(A11−A12A22−1A21)−1A12A22−1A22−1+A22−1A21(A11−A12A22−1A21)−1A12A22−1]
矩阵相等，意味着每一个元素都相等，得出下面矩阵求逆引理两个公式（四个里有重复）：
( A 11 − A 12 A 22 − 1 A 21 ) − 1 = A 11 − 1 + A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 A 21 A 11 − 1 A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 = ( A 11 − A 12 A 22 − 1 A 21 ) − 1 A 12 A 22 − 1 \begin{array}{l}\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1}=A_{11}^{-1}+A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} A_{21} A_{11}^{-1} \\ A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1}=\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1} A_{12} A_{22}^{-1}\end{array} (A11−A12A22−1A21)−1=A11−1+A11−1A12(A22−A21A11−1A12)−1A21A11−1A11−1A12(A22−A21A11−1A12)−1=(A11−A12A22−1A21)−1A12A22−1
回过头看递推最小二乘的公式：
{ P k = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 X ^ k = X ^ k − 1 + P k H k T R k − 1 ( Z k − H k X ^ k − 1 ) \left\{\begin{array}{l}\boldsymbol{P}_{k}=\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k-1}+\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right)\end{array}\right. ⎩⎨⎧Pk=(Pk−1−1+HkTRk−1Hk)−1X^k=X^k−1+PkHkTRk−1(Zk−HkX^k−1)
可以看出 P 矩阵的递推和矩阵求逆引理的第一个公式对应，令 A 11 = P k − 1 − 1 \boldsymbol{A}_{11}=\boldsymbol{P}_{k-1}^{-1} A11=Pk−1−1, A 12 = − H k T \quad \boldsymbol{A}_{12}=-\boldsymbol{H}_{k}^{\mathrm{T}} A12=−HkT, A 22 = R k \quad \boldsymbol{A}_{22}=\boldsymbol{R}_{k} A22=Rk, A 21 = H k \quad \boldsymbol{A}_{21}=\boldsymbol{H}_{k} A21=Hk ，则有：
P k = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 = P k − 1 − P k − 1 H k T ( R k + H k P k − 1 H k T ) − 1 H k P k − 1 \boldsymbol{P}_{k}=\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}=\boldsymbol{P}_{k-1}-{\color{red}\boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{R}_{k}+\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right)^{-1}}\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} Pk=(Pk−1−1+HkTRk−1Hk)−1=Pk−1−Pk−1HkT(Rk+HkPk−1HkT)−1HkPk−1
从左式到右式，看起来更复杂了，但其实左边要求逆三次，右边只要求逆一次。再仔细看，标红的部分与矩阵求逆引理的第二个公式对应：
P k − 1 H k T ( R k + H k P k − 1 H k T ) − 1 = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 H k T R k − 1 = P k H k T R k − 1 \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{R}_{k}+\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right)^{-1}={\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1}}=\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} Pk−1HkT(Rk+HkPk−1HkT)−1=(Pk−1−1+HkTRk−1Hk)−1HkTRk−1=PkHkTRk−1
带入变化之后式子就可以变得很简单，而且发现最后的结果与状态更新中新息向量 ( Z k − H k X ^ k − 1 ) \left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) (Zk−HkX^k−1) 前的系数一致，把红色的部分记为 K K K ，得最终公式：
{ K k = P k − 1 H k T ( H k P k − 1 H k T + R k ) − 1 X ^ k = X ^ k − 1 + K k ( Z k − H k X ^ k − 1 ) P k = ( I − K k H k ) P k − 1 \left\{\begin{array}{l}\boldsymbol{K}_{k}=\boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) \\ \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k-1}\end{array}\right. ⎩⎪⎪⎨⎪⎪⎧Kk=Pk−1HkT(HkPk−1HkT+Rk)−1X^k=X^k−1+Kk(Zk−HkX^k−1)Pk=(I−KkHk)Pk−1
状态的更新是上一时刻的状态 X ^ k − 1 \hat{\boldsymbol{X}}_{k-1} X^k−1 加上基于当前时刻量测进行的修正 K k ( Z k − H k X ^ k − 1 ) \boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) Kk(Zk−HkX^k−1) 。修正量是量测值与上一时刻的差值 ( Z k − H k X ^ k − 1 ) \left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) (Zk−HkX^k−1) 乘以增益系数 K k \boldsymbol{K}_{k} Kk。