卡尔曼滤波详细推导

卡尔曼滤波

背景

卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。
对于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):
x ~ k = x ˉ k + K ( Z k − H x ˉ k ) ; \tilde{x}_{k} = \bar{x}_k + K(Z_k-H\bar{x}_k); x~k​=xˉ