课程链接：https://www.bilibili.com/video/BV11K411J7gp/?p=1

参考书目：《捷联惯导算法与组合导航原理》-严恭敏、《卡尔曼滤波与组合导航原理》-秦永元、《Kalman滤波理论及其在导航系统中的应用》-付梦印、《惯性仪器测试与数据分析》-严恭敏

先修课程：矩阵论、数理统计（随机过程）、自动控制原理（现代控制）、数字信号处理（我都没学过，不影响看）

一、滤波的基本概念

1、传统数字滤波器

主要是基于频带来设计，单输入单输出系统。认为数字滤波器处理的是确定性信号，有用的信号在确定的频带内，要去除的噪声在频带之外；噪声在频带内，就没法处理

两种方法：①模拟转数字②直接数字滤波器

MATLAB工具箱fdatool，可以很方便的设置数字滤波器：

2、现代控制中的状态观测器

• 如果参数都知道，上下两部分就只差了一个观测器矩阵E，类似与Kalman滤波中的K矩阵
• 现代控制中处理的是确定性信号，可以用函数来描述，而估计器处理的是带误差的信号

3、最优估计的含义

每一个分量的二阶矩都达到最小：
$$\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$$
即：$\mathrm{E}\left[{\tilde{\mathbf{X}}}_k^{\mathrm{T}}{\tilde{\mathbf{X}}}_k\right]=\min$：
$$\mathrm{E}\left[{\tilde{\mathbf{X}}}_k{\tilde{\mathbf{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]\\ ⋮& ⋮& \ddots & ⋮\\ \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]$$
只需要关注对角线上的元素，即求迹：
$$\mathrm{tr}\left({\mathbf{P}}_k\right)=\mathrm{tr}\left(\mathrm{E}\left[{\tilde{\mathbf{X}}}_k{\tilde{\mathbf{X}}}_k^{\mathrm{T}}\right]\right)=\min$$

4、温度估计的例子

1.问题描述

• 某房间内温度受随机干扰影响——不恒定、波动
• 每小时用温度计测量一次温度——离散观测点
• 试对该房间温度作最佳估计——建模
• 干扰：$W\sim N\left(0,0.4^{\wedge }2\right)$——实际参数波动
• 温度计误差 ：$V\sim N\left(0,0.3^{\wedge }2\right)$——观测值噪声

2.分析

• 假设知道上一时刻温度 25℃，可以知道这一小时的温度受到干扰影响；根据高斯分布，67%概率在干扰的范围内 25±0.4℃，

• 如果温度计读数是 25.2℃，根据高斯分布，67%概率温度在 25.2±0.3℃

• 现在有两方面的信息，一个是惯性保持下来的 25±0.4℃，一个是量测得到的 25.2±0.3℃。如何得出此房间的温度？

• 按照最朴素的想法：两个值加权平均。0.4 代表误差大一些，0.3 代表误差小一些，量测信息更准确，占的权重更大。

• 假设两个值不相关，按概率论方法，可得到：

  

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

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

  

• 从噪声（方差）的角度看，这其实很类似电路，噪声往里加就像串联电路，量测信息和预测信息的结合就像是并联电路

• 如果干扰 W=0，则上一时刻温度是多少，下一时刻温度还是多少；房间是常温，温度计量测带误差，就相当于对常量进行估计，用递推最小二乘法。

• 如果量测噪声 V=0，温度计很准，量出来多少就是多少，变成确定性系统，不用估计了。

二、递推最小二乘

最小二乘量测模型：
$${\mathbf{Z}}_k={\mathbf{H}}_k\mathbf{X}+{\mathbf{V}}_k\mathrm{E}\left[{\mathbf{V}}_k\right]=0\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]={\mathbf{R}}_k{\delta }_{kj}$$

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

每一个观测时刻 $K$ ，都有一组量测方程 ${\mathbf{Z}}_k={\mathbf{H}}_k\mathbf{X}+{\mathbf{V}}_k$ 。一般认为设计矩阵 ${\mathbf{H}}_k$ 的列数 n 是确定的（代表参数个数不变），行数 $m$ 是不确定的（代表观测值个数可变）。可以把很多观测时刻的数据都列拼接在一起：
$${\overline{\mathbf{Z}}}_i=\left[\begin{array}{c}{\mathbf{Z}}_1\\ {\mathbf{Z}}_2\\ ⋮\\ {\mathbf{Z}}_i\end{array}\right],{\overline{\mathbf{H}}}_i=\left[\begin{array}{c}{\mathbf{H}}_1\\ {\mathbf{H}}_2\\ ⋮\\ {\mathbf{H}}_i\end{array}\right],{\overline{\mathbf{V}}}_i=\left[\begin{array}{c}{\mathbf{V}}_1\\ {\mathbf{V}}_2\\ ⋮\\ {\mathbf{V}}_i\end{array}\right]$$
当 $i=k-1$ 时刻，对此方程做加权最小二乘估计：
$${\hat{\mathbf{X}}}_{k-1}={\left({\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{H}}}_{k-1}\right)}^{-1}{\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{Z}}}_{k-1}={\mathbf{P}}_{k-1}{\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{Z}}}_{k-1}$$

上式利用到了前面所有时刻的观测值。同理当 $i=k$ 时的最小二乘估计：
$$\begin{array}{l}{\hat{\mathbf{X}}}_k={\left({\overline{\mathbf{H}}}_k^{\mathrm{T}}{\overline{\mathbf{R}}}_k^{-1}{\overline{\mathbf{H}}}_k\right)}^{-1}{\overline{\mathbf{H}}}_l^{\mathrm{T}}{\overline{\mathbf{R}}}_k^{-1}{\overline{\mathbf{Z}}}_k\\ ={\mathbf{P}}_k\left(\left[\begin{array}{ll}{\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}& {\mathbf{H}}_k^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}{\overline{\mathbf{R}}}_{k-1}^{-1}0& \\ 0& {\mathbf{R}}_k^{-1}\end{array}\right]\left[\begin{array}{c}{\overline{\mathbf{Z}}}_{k-1}\\ \${\mathbf{Z}}_k\end{array}\right]\right)\\ ={\mathbf{P}}_k\left({\overline{\mathbf{H}}}_{\mathrm{\Sigma }-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{Z}}}_{k-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_1^{-1}{\mathbf{Z}}_k\right)\end{array}$$
递推目标函数：知道前一时刻的估计值 ${\hat{\mathbf{X}}}_{k-1}$、${\mathbf{P}}_{k-1}$ 矩阵，和当前时刻测量得来的信息 ${\mathbf{H}}_k,{\mathbf{R}}_k,{\mathbf{Z}}_k$ ，求得当前时刻的 ${\mathbf{H}}_k,{\mathbf{R}}_k,{\mathbf{Z}}_k$
$$\left({\mathbf{H}}_k,{\mathbf{R}}_k,{\mathbf{Z}}_k\right)=f\left({\hat{\mathbf{X}}}_{k-1},{\mathbf{P}}_{k-1},{\mathbf{H}}_k,{\mathbf{R}}_k,{\mathbf{Z}}_k\right)$$
方差阵递推（协方差传播定律）：
$$\begin{array}{rl}{\mathbf{P}}_k& ={\left({\overline{\mathbf{H}}}_k^{\mathrm{T}}{\overline{\mathbf{R}}}_k^{-1}{\overline{\mathbf{H}}}_k\right)}^{-1}\\ & ={\left(\left[\begin{array}{ll}{\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}& {\mathbf{H}}_k^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}{\overline{\mathbf{R}}}_{k-1}^{-1}& 0\\ 0& {\mathbf{R}}_k^{-1}\end{array}\right]\left[\begin{array}{c}{\overline{\mathbf{H}}}_{k-1}\\ {\mathbf{H}}_k\end{array}\right]\right)}^{-1}\\ & ={\left({\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{H}}}_{k-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right)}^{-1}\\ & ={\left({\mathbf{P}}_{k-1}^{-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right)}^{-1}\end{array}$$
上式求逆太多，可以写为下面逆的形式：
$${\mathbf{P}}_k^{-1}={\mathbf{P}}_{k-1}^{-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k$$
状态估计递推：
$$\begin{array}{rl}{\hat{\mathbf{X}}}_k& ={\mathbf{P}}_k\left({\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{Z}}}_{k-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{Z}}_k\right)={\mathbf{P}}_k\left({\mathbf{P}}_{k-1}^{-1}{\mathbf{P}}_{k-1}{\overline{\mathbf{H}}}_{k-1}^{\mathrm{T}}{\overline{\mathbf{R}}}_{k-1}^{-1}{\overline{\mathbf{Z}}}_{k-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{Z}}_k\right)\\ & ={\mathbf{P}}_k\left({\mathbf{P}}_{k-1}^{-1}{\hat{\mathbf{X}}}_{k-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{Z}}_k\right)={\mathbf{P}}_k\left[\left({\mathbf{P}}_k^{-1}-{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right){\hat{\mathbf{X}}}_{k-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{Z}}_k\right]\\ & ={\hat{\mathbf{X}}}_{k-1}+{\mathbf{P}}_k{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k-1}\right)\end{array}$$
上式已经是递推最小二乘了，但为了更接近Kalman滤波，还可以继续向下推导。由于求逆特别多，引入矩阵求逆引理：

对非奇异矩阵 $\mathbf{A}$ 及子矩阵 ${\mathbf{A}}_{11},{\mathbf{A}}_{22}$, 若 $\mathbf{A}=\left[\begin{array}{ll}{\mathbf{A}}_{11}& {\mathbf{A}}_{12}\\ {\mathbf{A}}_{21}& {\mathbf{A}}_{22}\end{array}\right]$ ，把 $\mathbf{A}$ LU分解成求逆简单的三角阵，再求逆:
$$\mathbf{A}=\left[\begin{array}{cc}{\mathbf{I}}_n& 0\\ {\mathbf{A}}_{21}{\mathbf{A}}_{11}^{-1}& {\mathbf{I}}_m\end{array}\right]\left[\begin{array}{cc}{\mathbf{A}}_{11}& {\mathbf{A}}_{12}\\ 0& {\mathbf{A}}_{22}-{\mathbf{A}}_{21}{\mathbf{A}}_{11}^{-1}{\mathbf{A}}_{12}\end{array}\right]$$

$${\left[\begin{array}{cc}{\mathbf{I}}_n& 0\\ {\mathbf{A}}_{21}{\mathbf{A}}_{11}^{-1}& {\mathbf{I}}_m\end{array}\right]}^{-1}=\left[\begin{array}{cc}{\mathbf{I}}_n& 0\\ -{\mathbf{A}}_{21}{\mathbf{A}}_{11}^{-1}& {\mathbf{I}}_m\end{array}\right]$$

$${\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}\\ 0& {\left(A_{22}-A_{21}{A}_{11}^{-1}{A}_{12}\right)}^{-1}\end{array}\right]$$

求逆后的两式相乘得：
$$\begin{array}{rl}{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{array}$$
矩阵相等，意味着每一个元素都相等，得出下面矩阵求逆引理两个公式（四个里有重复）：
$$\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}$$

回过头看递推最小二乘的公式：
$$\{\begin{array}{l}{\mathbf{P}}_k={\left({\mathbf{P}}_{k-1}^{-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right)}^{-1}\\ {\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k-1}+{\mathbf{P}}_k{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k-1}\right)\end{array}$$
可以看出 P 矩阵的递推和矩阵求逆引理的第一个公式对应，令 ${\mathbf{A}}_{11}={\mathbf{P}}_{k-1}^{-1}$, ${\mathbf{A}}_{12}=-{\mathbf{H}}_k^{\mathrm{T}}$,${\mathbf{A}}_{22}={\mathbf{R}}_k$,${\mathbf{A}}_{21}={\mathbf{H}}_k$ ，则有：
$${\mathbf{P}}_k={\left({\mathbf{P}}_{k-1}^{-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right)}^{-1}={\mathbf{P}}_{k-1}-{\mathbf{P}}_{k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{R}}_k+{\mathbf{H}}_k{\mathbf{P}}_{k-1}{\mathbf{H}}_k^{\mathrm{T}}\right)}^{-1}{\mathbf{H}}_k{\mathbf{P}}_{k-1}$$
从左式到右式，看起来更复杂了，但其实左边要求逆三次，右边只要求逆一次。再仔细看，标红的部分与矩阵求逆引理的第二个公式对应：
$${\mathbf{P}}_{k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{R}}_k+{\mathbf{H}}_k{\mathbf{P}}_{k-1}{\mathbf{H}}_k^{\mathrm{T}}\right)}^{-1}={\left({\mathbf{P}}_{k-1}^{-1}+{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right)}^{-1}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}={\mathbf{P}}_k{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}$$
带入变化之后式子就可以变得很简单，而且发现最后的结果与状态更新中新息向量 $\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k-1}\right)$ 前的系数一致，把红色的部分记为 $K$ ，得最终公式：
$$\{\begin{array}{l}{\mathbf{K}}_k={\mathbf{P}}_{k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\\ {\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k-1}\right)\\ {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k-1}\end{array}$$
状态的更新是上一时刻的状态 ${\hat{\mathbf{X}}}_{k-1}$ 加上基于当前时刻量测进行的修正 ${\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k-1}\right)$ 。修正量是量测值与上一时刻的差值 $\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k-1}\right)$ 乘以增益系数 ${\mathbf{K}}_k$。