文章目录

• 卡尔曼滤波
• • 背景
  • 线性系统模型建立
  • 公式推导
  • 总结
  • • 预测
    • 更新

卡尔曼滤波

背景

卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。
对于Kalman Filter的理解，用过的都知道“黄金五条”公式，且通过“预测”与“更新”两个过程来对系统的状态进行最优估计，被广泛应用于估计问题中。

线性系统模型建立

对于一个线性系统来说，运动、观测模型如下
$$\{\begin{array}{rl}{\mathbf{X}}_{\mathbf{k}}& =A{\mathbf{X}}_{\mathbf{k}-1}+\mathbf{B}{\mathbf{u}}_{\mathbf{k}}+{\mathbf{w}}_{\mathbf{k}}\\ {\mathbf{Z}}_{\mathbf{k}}& =H{\mathbf{X}}_{\mathbf{k}}+{\mathbf{n}}_{\mathbf{k}}\end{array}$$
其中:$A$表示转移矩阵、${\mathbf{X}}_{\mathbf{k}}$表示系统状态、$B$表示控制矩阵\、${\mathbf{u}}_{\mathbf{k}}$表示输入，$w_k$表示输入高斯噪声，其均值为0,协方差矩阵为$P$,$P(w)\in N(0,P)$。${\mathbf{Z}}_{\mathbf{k}}$表示测量值、$H$表示观测矩阵、$n_k$表示测量的噪声平均值为0,协方差为$R$,$p(v)\in N(0,R)$。
对于大多数系统来讲，并不是严格意义上的线性时变系统，或者是系统结构参数存在不确定性，这个不确定性用过程噪声和估计噪声来表征。总之，引入$w_k\text{、}{n}_k$的主要目的是表征不确定性。

公式推导

对于状态估计算法来说，我们可以获得状态量的三个值:状态预测值(${\bar{x}}_k$)、最佳估计值(${\tilde{x}}_k$)以及真实值($x_k$)。
预测值是系统先验估计值，真实值是系统的测量值，最佳估计值是系统后验最大估计值，它通过一个卡尔曼增益$K$来调节预测值和真实值。
状态预测值为(黄金法则1):
$${\bar{x}}_k=A{\tilde{x}}_{k-1}+B{u}_k$$
从而得到更新方程(黄金法则2):
$${\tilde{x}}_k={\bar{x}}_k+K(Z_k-H{\bar{x}}_k);$$
我们将联立式(1)(3)得到:
$${\tilde{x}}_k={\bar{x}}_k+K(H{x}_k+n_k-H{\bar{x}}_k)$$
进一步得到:
$${\tilde{x}}_k={\bar{x}}_k+K(H{x}_k-H{\bar{x}}_k)+K{n}_k$$
经过变换可得:
$$x_k-{\tilde{x}}_k=x_k-{\bar{x}}_k-K(H{x}_k-H{\bar{x}}_k)-K{n}_k$$
我们定义:
${\bar{e}}_k=x_k-{\bar{x}}_k$表示真值与估计值之间的误差(先验)
$e_k=x_k-{\tilde{x}}_k$表示真值与最优估计值之间的误差（后验）
${\bar{P}}_k=E[{\bar{e}}_k{\bar{e}}_k^T]$表示真值与估计之间的协方差矩阵
$P_k=E[e_k{e}_k^T]$表示真值与估计之间的协方差矩阵
于是，式(6)转换为:
$$e_k=(I-KH){\bar{e}}_k-K{n}_k$$
于是，可以得到后验最优协方差矩阵:
$$P_k=E[e_k{e}_k^T]=(I-KH){\bar{P}}_k(I-KH{)}^T+KR{K}^T$$
式(8)展开得到:
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ P_k = \bar{P}_…
对于卡尔曼滤波来说，其目标是观测值与预测值之间的协方差最下，使得其接近真值：
$$J=\arg \underset{K}{\min }tr(P)$$
对于每一项$P_k$来说，对$K$求导:
$$\frac{\partial P_k}{\partial K}=-2{\bar{P}}_k{H}^T+2K(H{\bar{P}}_k{H}^T+R)=0$$
得到黄金法则3更新卡尔曼滤波的增益:
$$K={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+R{)}^{-1}$$
将式(11)带入式(8)中，得到黄金法则4更新后验最佳协方差:
$$P_k=(I-KH){\bar{P}}_k$$
可以通过这次状态递推得到下一阶段的先验估计值及先验误差值:
$$\begin{array}{rl}{\bar{e}}_{k+1}& =x_{k+1}-{\bar{x}}_{k+1}\\ & =A{x}_k+w_k-A\tilde{x}\\ & =A{\tilde{e}}_k+w_k\end{array}$$
然后可以更新下一次的先验协方差矩阵(黄金法则5):
$$\begin{array}{rl}{\bar{P}}_{k+1}& =E[{\bar{e}}_k{\bar{e}}_k^T]\\ & =E[(A{\tilde{e}}_k+w_k)(A{\tilde{e}}_k+w_k{)}^T]\\ & =E[(A{\tilde{e}}_k)(A{\tilde{e}}_k{)}^T]+E[w_k{w}_k^T]\\ & =A{P}_k{A}^T+Q\end{array}$$
至此我们递推出卡尔曼滤波的黄金五法则!

总结

从推导过程，我们可以看出，卡尔曼滤波分为两个部分：

预测

预测的过程就是根据上次的最优状态估计量，去估计下次的状态，这个估计往往是一个先验值：
$${\bar{x}}_k=A{\tilde{x}}_{k-1}+B{u}_k$$
并且，我们同样可以根据上次估计的协方差估计本次协方差:
$${\bar{P}}_k=A{P}_{k-1}{A}^T+Q$$
于是，我们就得到了关于先验的信息，接下来进行与观测值的加权过程(修正)

更新

首先，我们可以得到最新卡尔曼增益值:
$$K_k={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+R{)}^{-1}$$
然后我们用卡尔曼滤波进行对估计值进行修正:
$${\tilde{x}}_k={\bar{x}}_k+K_k(z_k-H{\bar{x}}_k)$$
最后更新最优估计的协方差矩阵:
$$P_k=(I-K_{k}H){\bar{P}}_k$$