卡尔曼滤波与组合导航原理笔记（一） 第一部分 滤波的基本概念、递推最小二乘

news2024/11/24 3:40:39

文章目录

    • 一、滤波的基本概念
      • 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~kT​X~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~k​X~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~k​X~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)]

    可以看出，一个值的权重取决于另一个值的误差，另一个值误差很小，这个权重就小

  • 再往下一时刻，算法也同样

    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-LVEaFehN-1685885499740)(卡尔曼滤波与组合导航原理（一）卡尔曼滤波方程的推导.assets/1685795612188.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​=Hk​X+Vk​E[Vk​]=0E[Vk​VjT​]=Rk​δkj​

状态方差也有，就是常值，状态转移矩阵就是单位阵，所以一般省略

每一个观测时刻 K K K ，都有一组量测方程 Z k = H k X + V k \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}+\boldsymbol{V}_{k} Zk​=Hk​X+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​= ​Z1​Z2​⋮Zi​​ ​,Hi​= ​H1​H2​⋮Hi​​ ​,Vi​= ​V1​V2​⋮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−1T​Rk−1−1​Hk−1​)−1Hk−1T​Rk−1−1​Zk−1​=Pk−1​Hk−1T​Rk−1−1​Zk−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​=(HkT​Rk−1​Hk​)−1HlT​Rk−1​Zk​=Pk​([Hk−1T​​HkT​​][Rk−1−1​00​Rk−1​​][Zk−1​$Zk​​])=Pk​(HΣ−1T​Rk−1−1​Zk−1​+HkT​R1−1​Zk​)​
递推目标函数：知道前一时刻的估计值 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​​=(HkT​Rk−1​Hk​)−1=([Hk−1T​​HkT​​][Rk−1−1​0​0Rk−1​​][Hk−1​Hk​​])−1=(Hk−1T​Rk−1−1​Hk−1​+HkT​Rk−1​Hk​)−1=(Pk−1−1​+HkT​Rk−1​Hk​)−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​+HkT​Rk−1​Hk​
状态估计递推：
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−1T​Rk−1−1​Zk−1​+HkT​Rk−1​Zk​)=Pk​(Pk−1−1​Pk−1​Hk−1T​Rk−1−1​Zk−1​+HkT​Rk−1​Zk​)=Pk​(Pk−1−1​X^k−1​+HkT​Rk−1​Zk​)=Pk​[(Pk−1​−HkT​Rk−1​Hk​)X^k−1​+HkT​Rk−1​Zk​]=X^k−1​+Pk​HkT​Rk−1​(Zk​−Hk​X^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=[A11​A21​​A12​A22​​] ，把 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=[In​A21​A11−1​​0Im​​][A11​0​A12​A22​−A21​A11−1​A12​​]

[ 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] [In​A21​A11−1​​0Im​​]−1=[In​−A21​A11−1​​0Im​​]

[ 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] [A11​0​A12​A22​−A21​A11−1​A12​​]−1=[A11−1​0​−A11−1​A12​(A22​−A21​A11−1​A12​)−1(A22​−A21​A11−1​A12​)−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−1​A12​(A22​−A21​A11−1​A12​)−1A21​A11−1​−(A22​−A21​A11−1​A12​)−1A21​A11−1​​−A11−1​A12​(A22​−A21​A11−1​A12​)−1(A22​−A21​A11−1​A12​)−1​]=[(A11​−A12​A22−1​A21​)−1−A22−1​A21​(A11​−A12​A22−1​A21​)−1​−(A11​−A12​A22−1​A21​)−1A12​A22−1​A22−1​+A22−1​A21​(A11​−A12​A22−1​A21​)−1A12​A22−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​−A12​A22−1​A21​)−1=A11−1​+A11−1​A12​(A22​−A21​A11−1​A12​)−1A21​A11−1​A11−1​A12​(A22​−A21​A11−1​A12​)−1=(A11​−A12​A22−1​A21​)−1A12​A22−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​+HkT​Rk−1​Hk​)−1X^k​=X^k−1​+Pk​HkT​Rk−1​(Zk​−Hk​X^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​+HkT​Rk−1​Hk​)−1=Pk−1​−Pk−1​HkT​(Rk​+Hk​Pk−1​HkT​)−1Hk​Pk−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−1​HkT​(Rk​+Hk​Pk−1​HkT​)−1=(Pk−1−1​+HkT​Rk−1​Hk​)−1HkT​Rk−1​=Pk​HkT​Rk−1​
带入变化之后式子就可以变得很简单，而且发现最后的结果与状态更新中新息向量 ( Z k − H k X ^ k − 1 ) \left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) (Zk​−Hk​X^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−1​HkT​(Hk​Pk−1​HkT​+Rk​)−1X^k​=X^k−1​+Kk​(Zk​−Hk​X^k−1​)Pk​=(I−Kk​Hk​)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​−Hk​X^k−1​) 。修正量是量测值与上一时刻的差值 ( Z k − H k X ^ k − 1 ) \left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) (Zk​−Hk​X^k−1​) 乘以增益系数 K k \boldsymbol{K}_{k} Kk​。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/610274.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

搭建本地MQTT服务器实现局域网通信

搭建本地MQTT服务器实现局域网通信

在这里mqtt就不多做介绍了直接上手怎么搭建本地服务器 一-. 我们先下载一个emqx，我们可以去官网下载免费的 https://www.emqx.io/https://www.emqx.io/ 下载完成之后我们打开这个文件夹 然后进入bin目录在左上角输入cmd然后回车 如果操作成功会来到这个界面 在这…
阅读更多...
不压缩打包layui

不压缩打包layui

手动打包 下载layui源码（当前版本2.6.4），并解压缩 下载地址：layui gitee 安装nodejs（v10.24.1） 下载链接 windows-x64 安装cnpm npm install -g cnpm -registryhttps://registry.npm.taobao.org全局安…
阅读更多...
Real3D 动画书 jQuery 插件

Real3D 动画书 jQuery 插件

Real3D FlipBook jQuery Plugin 插件 新 – 用于 REAL3D 动画书 JQUERY 插件的 PDF 到图像转换器 一种将 PDF 转换为图像和 JSON 的工具，用于创建带有链接、文本搜索和文本选择的优化 PDF 活页簿。 使用图像和 JSON 创建的 PDF 动画书加载页面的速度比标准 PDF 动画…
阅读更多...
i.MX6ULL点灯

i.MX6ULL点灯

i.MX6ULL点灯 对应底板原理图找到对应的IO引脚号 CCGR寄存器共有七个，分别是0~6。 使能时钟，CCGR0~6这7个寄存器控制着6ULL所有外设时钟的使能。 为了简单，设置CCGR0~6这7个寄存器全部为0xFFFFFFFF，相当于使能所有外设时钟。 …
阅读更多...
springboot项目外卖管理 day01-项目搭建以及后台登陆

springboot项目外卖管理 day01-项目搭建以及后台登陆

文章目录 一、软件开发整体介绍1.1、软件开发流程1.2、角色分工1.3、软件环境1.4、技术选型1.5、功能架构1.6、角色 二、环境搭建2.1、数据库的创建2.2、创建springboot项目并添加依赖2.3、配置yml文件2.4、将前端页面配置进resource目录![在这里插入图片描述](https://img-blo…
阅读更多...
ec-canvas 在小程序上的使用

ec-canvas 在小程序上的使用

文章目录 I. 前言echarts、ec-canvas，在小程序中进行数据可视化的意义 II. 安装ec-canvas1. 下载安装ec-canvas组件2. 配置组件参数 III. 初识ec-canvas1. echarts在微信小程序中的工作原理2. echarts小程序版的局限性与创新点3. 通过一个简单的示例了解ec-canvas的…
阅读更多...
chatgpt赋能python：Python备份列表l:保护你的重要数据

chatgpt赋能python：Python备份列表l:保护你的重要数据

Python备份列表l: 保护你的重要数据 当我们谈到数据的安全性时，备份是非常重要的。备份应该是在任何操作之前考虑的，因为在数据丢失或计算机崩溃时，我们需要在短时间内恢复数据。在这篇文章中，我们将讨论Python备份列表l。 什么…
阅读更多...
【python技能树】python简介

【python技能树】python简介

1 Python定义 Python 是一种简单易学并且结合了解释性、编译性、互动性和面向对象的脚本语言。Python提供了高级数据结构，它的语法和动态类型以及解释性使它成为广大开发者的首选编程语言。 Python 是解释型语言： 开发过程中没有了编译这个环节。类似于…
阅读更多...
Linux系统-Ubuntu安装指定版本的内核

Linux系统-Ubuntu安装指定版本的内核

Ubuntu安装指定版本的内核 以下演示 Linux 系统内核,手动安装的话可以安装所有指定版本的内核。 查看当前系统内核 uname -sr接下来以安装 5.13.0 内核为例 首先去 http://kernel.ubuntu.com/~kernel-ppa/mainline/找到内核版本为5.13.0 的链接 根据电脑64位处理器 选择 AM…
阅读更多...
chatgpt赋能python：Python奇数和偶数和

chatgpt赋能python：Python奇数和偶数和

Python奇数和偶数和 在Python编程中，奇数和偶数和是一个基本的概念。奇数和偶数是指整数的特定类型，其中奇数是指不能被2整除的正整数，偶数是指可以被2整除的正整数。本文将介绍Python中计算奇数和偶数和的方法。 计算奇数和偶数 要计算奇…
阅读更多...
「QT」QT5程序设计目录

「QT」QT5程序设计目录

✨博客主页：何曾参静谧的博客 📌文章专栏：「QT」QT5程序设计 目录 📑【QT的基础知识篇】📑【QT的GUI编程篇】📑【QT的项目示例篇】📑【QT的网络编程篇】📑【QT的数据库编程篇】📑【QT的跨平台编程篇】📑【QT的高级编程篇】📑【QT的开发工具篇】📑【QT的调…
阅读更多...
chatgpt赋能python：Python地址怎么写的SEO

chatgpt赋能python：Python地址怎么写的SEO

Python地址怎么写的SEO 随着数字化时代的到来，搜索引擎优化（SEO）已成为网站排名和流量的重要因素。为了让Python 相关网站在搜索引擎上更加有利的排名，需要了解Python地址要怎么写才能获得更好的SEO效果。 为什么地址要写好 地…
阅读更多...
操作系统（进程与线程，复习自用）

操作系统（进程与线程，复习自用）

进程与线程 进程与线程进程的概念、组成、特征进程的状态与转换进程控制进程通信（IPC）线程概念 多线程模型线程的实现方式 多线程模型线程的状态与转换 CPU调度与上下文切换调度的概念、层次进程调度的时机 切换与过程调度方式调度器 闲逛进程调度算法的…
阅读更多...
chatgpt赋能python：Python备份交换机：保障网络安全的必备工具

chatgpt赋能python：Python备份交换机：保障网络安全的必备工具

Python备份交换机：保障网络安全的必备工具 在当今互联网时代，因特网已经成为了全球的必备基础设施。然而，在使用网络的过程中，我们也会遭遇到各种各样的问题，其中最常见的就是网络中断或数据丢失。特别是对于企业和组…
阅读更多...
javaScript蓝桥杯----绝美宋词

javaScript蓝桥杯----绝美宋词

目录 一、介绍二、准备三、目标四、代码五、踩坑六、完成 一、介绍 “今宵酒醒何处，杨柳岸晓风残月”，“蓦然回首，那人却在灯火阑珊处”，“试问闲愁都几许？一川烟草，满城风絮，梅子黄时雨” … …
阅读更多...
K8s网络管理 flannel

K8s网络管理 flannel

K8s网络管理 flannel 1 网络管理1.1 Service1.1.1 网络体系1.1.2 工作模型1.1.3 SVC实践1.1.4 IPVS实践 1.2 其他资源1.2.1 域名服务1.2.2 CoreDNS1.2.3 无头服务 2 容器网络2.1 网络方案2.2 flannel2.3 主机网络 1 网络管理 1.1 Service 1.1.1 网络体系 应用流程 资源对象…
阅读更多...
Elasticsearch基础

Elasticsearch基础

Elasticsearch是搜索服务器 视频地址：https://www.bilibili.com/video/BV1Sy4y1G7LL/?p6&spm_id_from333.880.my_history.page.click&vd_sourcefc7fa697b97f292319c5b0cde4755484 下载地址：[https://artifacts.elastic.co/downloads/elasticse…
阅读更多...
赋予品牌文化原力，从已有文化中挖掘

赋予品牌文化原力，从已有文化中挖掘

赋予品牌以文化原力，从已有文化中挖掘 华与华总结为： 寻找母体，寄生母体，成为母体，壮大母体 非常经典的例子：微信红包 趣讲大白话：文化有持续的力量 【趣讲信息科技186期】 *******************…
阅读更多...
Redisson看门狗机制为什么比将锁设置成永久有效期更好？

Redisson看门狗机制为什么比将锁设置成永久有效期更好？

个人主页：金鳞踏雨 个人简介：大家好，我是金鳞，一个初出茅庐的Java小白 目前状况：22届普通本科毕业生，几经波折了，现在任职于一家国内大型知名日化公司，从事Java开发工作 我的博客&am…
阅读更多...
【测绘程序设计】C#伪距单点定位

【测绘程序设计】C#伪距单点定位

文章目录 一、题目解读二、界面设计三、矩阵计算实现1、矩阵定义Matrix2、矩阵构造Matrix()3、单位矩阵MatrixE()4、加减乘操作符重载-*5、矩阵转置transposs()6、矩阵求逆Inverse() 四、数据存储结构设计1、Sat类存一颗卫星的数据2、Epoch类存一个历元的数据3、DataCenter类存…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023