02-卡尔曼滤波原理（二）卡尔曼滤波方程的推导.md

[toc]

三、卡尔曼滤波

1、随机系统状态空间模型

$$ \left{\begin{array}{l}\boldsymbol{X}{k}=\boldsymbol{\Phi}{k / k-1} \boldsymbol{X}{k-1}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1} \ \boldsymbol{Z}{k}=\boldsymbol{H}{k} \boldsymbol{X}{k}+\boldsymbol{V}_{k}\end{array}\right. $$

关于噪声的基本假设：系统噪声 $\boldsymbol{W}{k}$ 和量测噪声 $\boldsymbol{V}{k}$ 都是零均值高斯白噪声，且不相关；系统噪声协方差阵 $\boldsymbol{Q}{k}$ 对称非负定，观测噪声协方差阵 $\boldsymbol{R}{k}$ 对称正定。

• 其实改系统噪声前面系数 $\Gamma_{k}$ 可以使 $\boldsymbol{Q}_{k}$ 总正定。
• 必须是高斯白噪声，多个高斯白噪声线性组合还是高斯白噪声，零均值均匀分布噪声线性组合不一定还是零均值均匀分布噪声。

$$ \begin{array}{l}\left{\begin{array}{l}\mathrm{E}\left[\boldsymbol{W}{k}\right]=\mathbf{0}, \quad \mathrm{E}\left[\boldsymbol{W}{k} \boldsymbol{W}{j}^{\mathrm{T}}\right]=\boldsymbol{Q}{k} \delta_{k j} \ \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} \ \mathrm{E}\left[\boldsymbol{W}{k} \boldsymbol{V}{j}^{\mathrm{T}}\right]=\mathbf{0}\end{array}\right. \ \boldsymbol{Q}{k} \geq 0, \quad \boldsymbol{R}{k}>0，或\boldsymbol{Q}{k} > 0, \quad \boldsymbol{R}{k}>0 \end{array} $$

2、状态预测

状态一步预测：预测下一时刻最有可能的那个值，由于高斯白噪声对状态预测的一阶矩不起作用（线性最小方差估计）： $$ \hat{\boldsymbol{X}}{k / k-1}={\boldsymbol{\Phi}}{k / k-1} \hat{\boldsymbol{X}}{k-1} $$ 状态预测误差：真值-预测 $$ \begin{array}{r}\tilde{\boldsymbol{X}}{k / k-1}=\boldsymbol{X}{k}-\hat{\boldsymbol{X}}{k / k-1}=\left(\boldsymbol{\Phi}{k / k-1} \boldsymbol{X}{k-1}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1}\right)-\boldsymbol{\Phi}{k / k-1} \hat{\boldsymbol{X}}{k-1} \ \quad=\boldsymbol{\Phi}{k / k-1}\left(\boldsymbol{X}{k-1}-\hat{\boldsymbol{X}}{k-1}\right)+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1}=\boldsymbol{\Phi}{k / k-1} \tilde{\boldsymbol{X}}{k-1}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1}\end{array} $$ 状态预测方差阵：一步预测误差相乘的期望，乘出来有四项，由于两时刻噪声不相关，其中两项为0。所以预测的方差就是上一时刻的方差传递过来 $\boldsymbol{\Phi}{k / k-1} \boldsymbol{P}{k-1} \boldsymbol{\Phi}{k / k-1}^{\mathrm{T}}$，加上当前误差 $\boldsymbol{\Gamma}{k-1} \boldsymbol{Q}{k-1} \boldsymbol{\Gamma}{k-1}^{\mathrm{T}}$ $$ \begin{aligned} \boldsymbol{P}{k / k-1} & =\mathrm{E}\left[\tilde{\boldsymbol{X}}{k / k-1} \tilde{\boldsymbol{X}}{k / k-1}^{\mathrm{T}}\right] \ & =\mathrm{E}\left[\left(\boldsymbol{\Phi}{k / k-1} \tilde{\boldsymbol{X}}{k-1}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1}\right)\left(\boldsymbol{\Phi}{k / k-1} \tilde{\boldsymbol{X}}{k-1}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1}\right)^{\mathrm{T}}\right] \ & =\boldsymbol{\Phi}{k / k-1} \mathrm{E}\left[\tilde{\boldsymbol{X}}{k-1} \tilde{\boldsymbol{X}}{k-1}^{\mathrm{T}}\right] \boldsymbol{\Phi}{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}{k-1} \mathrm{E}\left[\boldsymbol{W}{k-1} \boldsymbol{W}{k-1}^{\mathrm{T}}\right] \boldsymbol{\Gamma}{k-1}^{\mathrm{T}} \ & =\boldsymbol{\Phi}{k / k-1} \boldsymbol{P}{k-1} \boldsymbol{\Phi}{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}{k-1} \boldsymbol{Q}{k-1} \boldsymbol{\Gamma}{k-1}^{\mathrm{T}} \end{aligned} $$

3、状态量测

量测：量测值带入量测方程，同样忽略高斯白噪声的影响： $$ \hat{\boldsymbol{Z}}{k / k-1}=\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1} $$ 量测误差： $$ \begin{aligned} \tilde{\boldsymbol{Z}}{k / k-1} & =\boldsymbol{Z}{k}-\hat{\boldsymbol{Z}}{k / k-1}=\left(\boldsymbol{H}{k} \boldsymbol{X}{k}+\boldsymbol{V}{k}\right)-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1} =\boldsymbol{H}{k} \tilde{\boldsymbol{X}}{k / k-1}+\boldsymbol{V}{k}\end{aligned} $$ 量测方差阵： $$ \begin{aligned} \boldsymbol{P}{Z Z, k / k-1} & =\mathrm{E}\left[\tilde{\boldsymbol{Z}}{k / k-1} \tilde{\boldsymbol{Z}}{k / k-1}^{\mathrm{T}}\right] \ & =\mathrm{E}\left[\left(\boldsymbol{H}{k} \tilde{\boldsymbol{X}}{k / k-1}+\boldsymbol{V}{k}\right)\left(\boldsymbol{H}{k} \tilde{\boldsymbol{X}}{k / k-1}+\boldsymbol{V}{k}\right)^{\mathrm{T}}\right] \ & =\boldsymbol{H}{k} \mathrm{E}\left[\tilde{\boldsymbol{X}}{k / k-1} \tilde{\boldsymbol{X}}{k / k-1}^{\mathrm{T}}\right] \boldsymbol{H}{k}^{\mathrm{T}}+\mathrm{E}\left[\boldsymbol{V}{k} \boldsymbol{V}{k}^{\mathrm{T}}\right] \ & =\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\end{aligned} $$ 量测互协方差：X 与 Z 之间，状态的一步预测误差和量测之间的互协方差

一般不为0，为0就没有滤波的必要了，量测值直接就是估计值

$$ \begin{array}{l}\boldsymbol{P}{X Z, k / k-1}=\mathrm{E}\left[\tilde{\boldsymbol{X}}{k / k-1} \tilde{\boldsymbol{Z}}{k / k-1}^{\mathrm{T}}\right] \ \quad=\mathrm{E}\left[\tilde{\boldsymbol{X}}{k / k-1}\left(\boldsymbol{H}{k} \tilde{\boldsymbol{X}}{k / k-1}+\boldsymbol{V}{k}\right)^{\mathrm{T}}\right] \ \quad=\boldsymbol{P}{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\end{array} $$

4、增益矩阵K与状态估计

假设滤波方程可以写成 状态预测+K量测修正 形式，${\color{red}K}$ 矩阵先待定

状态估计： $$ \hat{\boldsymbol{X}}{k}=\hat{\boldsymbol{X}}{k / k-1}+{\color{red}\boldsymbol{K}{k} }\tilde{{\boldsymbol{Z}}{k}}=\hat{\boldsymbol{X}}{k / k-1}+{\color{red}\boldsymbol{K}{k}}\left(\boldsymbol{Z}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1}\right) $$ 状态估计误差： $$ \begin{aligned} \tilde{\boldsymbol{X}}{k} & =\boldsymbol{X}{k}-\hat{\boldsymbol{X}}{k}=\boldsymbol{X}{k}-\left[\hat{\boldsymbol{X}}{k / k-1}+{\color{red}\boldsymbol{K}{k}}\left(\boldsymbol{Z}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1}\right)\right] \ & =\tilde{\boldsymbol{X}}{k / k-1}-{\color{red}\boldsymbol{K}{k}}\left(\boldsymbol{H}{k} \boldsymbol{X}{k}+\boldsymbol{V}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1}\right)=\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \tilde{\boldsymbol{X}}{k / k-1}-{\color{red}\boldsymbol{K}{k}} \boldsymbol{V}{k}\end{aligned} $$ 状态估计方差阵： $$ \begin{aligned} \boldsymbol{P}{k} &= \mathrm{E}\left[\tilde{\boldsymbol{X}}{k} \tilde{\boldsymbol{X}}{k}^{\mathrm{T}}\right] \ & =\mathrm{E}\left{\left[\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}{k}} \boldsymbol{H}{k}\right) \tilde{\boldsymbol{X}}{k / k-1}-{\color{red}\boldsymbol{K}{k}} \boldsymbol{V}{k}\right]\left[\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \tilde{\boldsymbol{X}}{k / k-1}-{\color{red}\boldsymbol{K}{k} }\boldsymbol{V}{k}\right]^{\mathrm{T}}\right} \ & =\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}{k}}\boldsymbol{H}{k}\right) \mathrm{E}\left[\tilde{\boldsymbol{X}}{k / k-1} \tilde{\boldsymbol{X}}{k / k-1}^{\mathrm{T}}\right]\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}{k}} \boldsymbol{H}{k}\right)^{\mathrm{T}}+{\color{red}\boldsymbol{K}{k}} \mathrm{E}\left[\boldsymbol{V}{k} \boldsymbol{V}{k}^{\mathrm{T}}\right] {\color{red}\boldsymbol{K}{k}^{\mathrm{T}}} \ & =\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}{k}} \boldsymbol{H}{k}\right) \boldsymbol{P}{k / k-1}\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}{k}} \boldsymbol{H}{k}\right)^{\mathrm{T}}+{\color{red}\boldsymbol{K}{k}} \boldsymbol{R}{k} {\color{red}\boldsymbol{K}_{k}^{\mathrm{T}}}\end{aligned} $$ 到此，都是假设，在待定 ${\color{red}K}$ 的情况下进行状态预测，并算出方差阵，下面就是要求${\color{red}K}$ ：

${\color{red}K}$ 矩阵可以取某个值，使状态估计方差 $\boldsymbol{P}{k}$ 达到最小，且保持 $\boldsymbol{P}{k}$ 对称正定。

要理解矩阵最小的含义，得先理解 $A$ 矩阵小于 $B$ 矩阵的含义：$B-A$ 正定

最优估计的含义： 每一个分量的二阶矩都达到最小： $$ \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{\boldsymbol{X}}{k}^{\mathrm{T}} \tilde{\boldsymbol{X}}{k}\right]=\min$： $$ \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] $$ 只需要关注对角线上的元素，即求迹： $$ \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 $$

由最优估计的含义可知，要得到最优估计等价于求 $\boldsymbol{P}{k}$ 的迹，最小，先展开： $$ \begin{aligned} \boldsymbol{P}{k} & =\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \boldsymbol{P}{k / k-1}\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right)^{\mathrm{T}}+\boldsymbol{K}{k} \boldsymbol{R}{k} \boldsymbol{K}{k}^{\mathrm{T}} \ & =\boldsymbol{P}{k / k-1}-\boldsymbol{K}{k} \boldsymbol{H}{k} \boldsymbol{P}{k / k-1}-\left(\boldsymbol{K}{k} \boldsymbol{H}{k} \boldsymbol{P}{k / k-1}\right)^{\mathrm{T}}+\boldsymbol{K}{k}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right) \boldsymbol{K}{k}^{\mathrm{T}}\end{aligned} $$ 再求迹，分解开，就是一个多输入单输出的多元函数： $$ \begin{aligned} \operatorname{tr}\left(\boldsymbol{P}{k}\right)= & \operatorname{tr}\left(\boldsymbol{P}{k / k-1}\right)-\operatorname{tr}\left(\boldsymbol{K}{k} \boldsymbol{H}{k} \boldsymbol{P}{k / k-1}\right) \ & -\operatorname{tr}\left(\left(\boldsymbol{K}{k} \boldsymbol{H}{k} \boldsymbol{P}{k / k-1}\right)^{\mathrm{T}}\right)+\operatorname{tr}\left(\boldsymbol{K}{k}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right) \boldsymbol{K}{k}^{\mathrm{T}}\right)\end{aligned} $$ 要求多元函数的极值，就是找求导等于 $0$ 的点，由求导规则： $$ \begin{aligned}\frac{\mathrm{d}}{\mathrm{d} \boldsymbol{X}}[\operatorname{tr}(\boldsymbol{X} \boldsymbol{B})]=\frac{\mathrm{d}}{\mathrm{d} \boldsymbol{X}}\left[\operatorname{tr}\left((\boldsymbol{X} \boldsymbol{B})^{\mathrm{T}}\right)\right]=\boldsymbol{B}^{\mathrm{T}} \ \frac{\mathrm{d}}{\mathrm{d} \boldsymbol{X}}\left[\operatorname{tr}\left(\boldsymbol{X} \boldsymbol{A} \boldsymbol{X}^{\mathrm{T}}\right)\right]=2 \boldsymbol{X} \boldsymbol{A}，A为对称阵时\end{aligned} $$ 求导得： $$ \frac{\mathrm{d}}{\mathrm{d} \boldsymbol{K}{k}}\left[\operatorname{tr}\left(\boldsymbol{P}{k}\right)\right]=\mathbf{0}{\color{green}-\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1}\right)^{\mathrm{T}}-\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1}\right)^{\mathrm{T}}}+2 {\color{red}\boldsymbol{K}{k}}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right)=\mathbf{0} $$ 绿色的部分为移到左边，两边消去 2 得： $$ \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}={\color{red}\boldsymbol{K}{k}}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right) $$

要求出 ${\color{red}K}$ ，需要 ${\color{red}K}$ 后面部分可逆，即量测噪声 $\boldsymbol{R}{k}$ 要正定；前面的 $\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}$ 由于 $\boldsymbol{H}{k}$ 有很多 $0$ 元素，可逆会不正定，如果加上一个总是正定 $\boldsymbol{R}{k}$，还能维持整体的正定，继续求逆。

${\color{red}K}$ 后面部分可逆乘到两边： $$ {\color{red}\boldsymbol{K}{k}}=\boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} $$ 将求出的增益矩阵 $K$ 带回到最上面状态估计和方差的的方程中，得出Kalman滤波解。

消除一部分元素，可以得到短一些的方差：$\boldsymbol{P}{k}=\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \boldsymbol{P}{k / k-1}$

5、Kalman滤波公式汇总

1. 状态一步预测：$\hat{\boldsymbol{X}}{k / k-1}=\boldsymbol{\Phi}{k / k-1} \hat{\boldsymbol{X}}_{k-1}$

2. 状态一步预测均方误差：$\boldsymbol{P}{k / k-1}=\boldsymbol{\Phi}{k / k-1} \boldsymbol{P}{k-1} \boldsymbol{\Phi}{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}{k-1} \boldsymbol{Q}{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}$

3. 滤波增益：$\boldsymbol{K}{k}=\boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1}$

4. 状态估计：$\hat{\boldsymbol{X}}{k}=\hat{\boldsymbol{X}}{k / k-1}+\boldsymbol{K}{k}\left(\boldsymbol{Z}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1}\right)$

5. 状态估计均方误差：$\boldsymbol{P}{k}=\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \boldsymbol{P}{k / k-1}$

6、Kalman滤波流程图

1.划分为左右两部分（一阶矩和二阶矩）

• 左边滤波计算回路是一阶矩的更新过程
• 右边增益计算回路是二阶矩的更新过程
• 左边对右边没有影响，右边通过增益矩阵对左边产生影响。有时候数值不好，左边计算都崩溃了，对右边都没有影响，还按照模型来计算，算出来的方差值还是很小的，反应不出真实的误差情况

2.划分为上下两部分（时间更新、量测更新）

• 上面是时间更新，只和状态方程有关系。
• 下面是量测更新，只和量测方程有关系。

7、Kalman滤波计算系统回路图

• 总的来说，是一个线性系统，输入 $Z_{k}$，输出 $\hat{{X}}_{k}$
• 右边部分是时间更新，以系统上一时刻的输出为输入，输出当前时刻的预测状态
• 左边部分是量测更新
• Kalman滤波对线性系统来说，是最小方差线性无偏估计

8、Kalman编程计算流程图

• 如果不是定常系统，$\boldsymbol{\Phi}{k / k-1}, \boldsymbol{\Gamma}{k-1}$,$\boldsymbol{Q}{k-1}$,$\boldsymbol{H}{k}, \boldsymbol{R}_{k}$ 都是时变的，需要给出；定常系统作为常数就行。
• 如果系统非线性，是高阶系统，必须给出初始状态 $\hat{\boldsymbol{X}}{0} ,\boldsymbol{P}{0}$
• 输出一阶矩、二阶矩
  • 一阶矩、二阶矩能代表系统的全部，进行Kalman滤波的随机信号需要是正态分布的
  • 二阶矩是为了下一时刻计算权重（增益矩阵 $K$）
• 量测更新和时间更新可能不同步；常见的是量测更新频率小于时间更新频率

9、Kalman滤波特性

1.状态估计时量测的线性组合

Kalman滤波是一种递推的公式，，上一时刻可以递推当前时刻，不断递推。以预测和量测加权的方式表示： $$ \begin{aligned} \hat{\boldsymbol{X}}{k} & =\hat{\boldsymbol{X}}{k / k-1}+\boldsymbol{K}{k}\left(\boldsymbol{Z}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1}\right) \ & =\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \boldsymbol{\Phi}{k / k-1} \hat{\boldsymbol{X}}{k-1}+\boldsymbol{K}{k} \boldsymbol{Z}{k}=\boldsymbol{G}{k} \hat{\boldsymbol{X}}{k-1}+\boldsymbol{K}{k} \boldsymbol{Z}{k} \ & =\boldsymbol{G}{k}\left(\boldsymbol{G}{k-1} \hat{\boldsymbol{X}}{k-2}+\boldsymbol{K}{k-1} \boldsymbol{Z}{k-1}\right)+\boldsymbol{K}{k} \boldsymbol{Z}{k} \ & =\boldsymbol{G}{k} \boldsymbol{G}{k-1} \hat{\boldsymbol{X}}{k-2}+\boldsymbol{G}{k} \boldsymbol{K}{k-1} \boldsymbol{Z}{k-1}+\boldsymbol{K}{k} \boldsymbol{Z}{k} \ & =\boldsymbol{G}{k} \boldsymbol{G}{k-1} \boldsymbol{G}{k-2} \hat{\boldsymbol{X}}{k-3}+\boldsymbol{G}{k} \boldsymbol{G}{k-1} \boldsymbol{K}{k-2} \boldsymbol{Z}{k-2}+\boldsymbol{G}{k} \boldsymbol{K}{k-1} \boldsymbol{Z}{k-1}+\boldsymbol{K}{k} \boldsymbol{Z}{k} \ & =\cdots \ & =\left(\prod_{i=1}^{k} \boldsymbol{G}{i}\right) \hat{\boldsymbol{X}}{0}+\sum_{i=1}^{k}\left(\prod_{j=i+1}^{k} \boldsymbol{G}{j}\right) \boldsymbol{K}{i} \boldsymbol{Z}{i}\end{aligned} $$ 如果初值 $\hat{\boldsymbol{X}}{0}$ 取值 $0$ ，当前的估计值就是前面所有量测值 $\boldsymbol{Z_i}$ 的线性组合

2.正交投影性质

• Kalman滤波估计就是预测和量测的加权平均。
• 量测是一条矢量、预测也是一条矢量，前面乘上加权系数再相加。
• 无论前面系数是多少，相加得到的估计值都在同一个平面上。
• 真实的状态也是空间中一条矢量。
• 要使估计值和真值直接的误差最小，即在平面上取到平面外一点距离最小的点，就是取正交投影点。

3.新息向量是零矩阵白噪声序列

$$ \left{\begin{array}{l}\mathrm{E}\left[\tilde{\boldsymbol{Z}}{k / k-1}\right]=\mathbf{0} \ \mathrm{E}\left[\tilde{\boldsymbol{Z}}{k / k-1} \tilde{\boldsymbol{Z}}{j / j-1}^{\mathrm{T}}\right]=\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right) \delta{k j}\end{array}\right. $$

10、带确定性输入的Kalman滤波

状态空间模型： $$ \left{\begin{array}{l}\boldsymbol{X}{k}=\boldsymbol{\Phi}{k / k-1} \boldsymbol{X}{k-1}+{\color{red}\boldsymbol{B}{k-1} \boldsymbol{u}{k-1}}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1} \ \boldsymbol{Z}{k}=\boldsymbol{H}{k} \boldsymbol{X}{k}+{\color{red}\boldsymbol{Y}{k}}+\boldsymbol{V}{k}\end{array}\right. $$ 滤波方程，只有和一阶矩有关的部分需要改动： $$ \left{\begin{array}{l}\hat{\boldsymbol{X}}{k / k-1}=\boldsymbol{\Phi}{k / k-1} \hat{\boldsymbol{X}}{k-1}+{\color{red}\boldsymbol{B}{k-1} \boldsymbol{u}{k-1}} \ \boldsymbol{P}{k / k-1}=\boldsymbol{\Phi}{k / k-1} \boldsymbol{P}{k-1} \boldsymbol{\Phi}{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}{k-1} \boldsymbol{Q}{k-1} \boldsymbol{\Gamma}{k-1}^{\mathrm{T}} \ \boldsymbol{K}{k}=\boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}\left(\boldsymbol{H}{k} \boldsymbol{P}{k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right)^{-1} \ \hat{\boldsymbol{X}}{k}=\hat{\boldsymbol{X}}{k / k-1}+\boldsymbol{K}{k}\left(\boldsymbol{Z}{k}-{\color{red}\boldsymbol{Y}{k}}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{k / k-1}\right) \ \boldsymbol{P}{k}=\left(\boldsymbol{I}-\boldsymbol{K}{k} \boldsymbol{H}{k}\right) \boldsymbol{P}{k / k-1}\end{array}\right. $$