卡尔曼滤波记录一 概念及推导

卡尔曼滤波理解

关于卡尔曼滤波的理解
核心是从多种不确定的信息中提取出有价值的信息，进而得到一个更确定的结果。

注：在概率论和统计学中，协方差用于衡量两个变量的总体误差。而方差是协方差的一种特殊情况，即当两个变量是相同的情况。

卡尔曼滤波公式推导

$$x_k=A\ast x_{k-1}+B\ast u_k+w_{k-1}\text{\hspace{1em}(1)}$$$$z_k=H\ast x_k+v_k\text{\hspace{1em}(2)}$$
其中，各变量表征的意义为：
$x_k$ 系统状态矩阵，-------，$z_k$ 状态阵的观测量（实测）

$A$ 状态转移矩阵 ，-------， $B$ 控制输入矩阵

$H$ 状态观测矩阵

$w_{k-1}$过程噪声，-------，$v_k$ 测量噪声
如果大家学过《现代控制理论》的话，对上述模型的描述形式一定不会陌生，只是多了变量$w_{k-1}$与$v_k$ 。其中，$w_{k-1}$ 代表过程噪声，$v_k$代表测量噪声，且为高斯白噪声，协方差分别为$Q$和$R$。

对于状态估计算法而言，我们可以获取状态量的三个值：状态预测值（$x_k^{-}$）、最优估计值（${\tilde{x}}_k$）以及真实值（$x_k$）,卡尔曼滤波的原理就是利用卡尔曼增益来修正状态预测值，使其逼近真实值。
卡尔曼滤波的推到分为两个过程：
１、状态估计协方差$p_k$ 的推导，即代价函数的求取；
２、卡尔曼增益矩阵及其他准则的推导；

1.1 状态估计协方差$p_k$
$x_k$状态的真实值；

${\tilde{x}}_k^{-}$ 状态的预测值，也称先验状态估计值（a prior state estimate）；

${\tilde{x}}_k$ 状态的最优估计值，也称后验状态估计值（a posteriori state estimate）；

---

状态预测值 ${\tilde{x}}_k^{-}$ 由状态预测方程可得：黄金1条
$${\tilde{x}}_k^{-}=A\ast {\tilde{x}}_{k-1}^{-}+B\ast u_k\text{\hspace{1em}(3)}$$状态最优估计值${\tilde{x}}_k$ 可由状态更新方程可得：黄金2条
$${\tilde{x}}_k={\tilde{x}}_k^{-}+K(z_k-H\ast {\tilde{x}}_k^{-})\text{\hspace{1em}(4)}$$通过该方程可知，卡尔曼增益$K$ 实际上表征了状态最优估计过程中模型预测误差（Predicted error）与量测误差（Measurement error）的比重（如下所示），即$k\in [0,1]$。当 $k=0$时，即预测误差为0，系统的状态值完全取决于预测值（${\tilde{x}}_k={\tilde{x}}_k^{-}$ ）；而当 $k=\mathrm{１}$ 时，即量测误差为0，系统的状态值完全取决于量侧值。
$$k=Predictederror/(Predictederror+Measurementerror)$$因此，可令：

其中：