卡尔曼滤波公式理解

卡尔曼滤波

卡尔曼滤波适用于线性高斯系统，即系统满足叠加性、齐次性，噪声满足正态分布。其使用上一次的最优结果预测当前的值（先验估计），同时使用观测值修正当前值，得到最优结果。

卡尔曼、粒子滤波实际应该叫做估计器（Estimator），估计当前值叫滤波（Filtering），估计过去叫平滑（Smoothing），估计未来叫预测（Predicting）。

卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。

Kalman Filter

1. 状态空间模型

卡尔曼滤波假设系统状态与噪声是高斯的，可以用均值和方差来描述。则上述状态空间模型可以被表述为：

• 系统状态方程

  $$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$$

  • $x_k$ 是状态分量的 n 维矢量
  • $A$ 是 $n\times n$ 的状体转移矩阵，也就是目标状态转移的猜想模型，是已知的
  • $u_{k-1}$是新的，让系统可以接受外部控制
  • $B$ 是 $n\times c$ 的矩阵，将输入转化为状态的矩阵
  • $w_{k-1}$ 是预测过程中服从高斯分布的噪声，对应了$x_k$ 中每个分量的噪声，期望为0，协方差为 Q 的高斯白噪声 $w_{k-1}\sim N(0,Q)$, Q 即下文过程激励噪声。

• 系统观测方程
  $$z_k=H{x}_k+v_k$$

  • $H$ 是$m\times n$ 矩阵，是状态变量（观测）的转移矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将m维的测量值转换到 $n$ 维，使之符合状态变量的数学形式。
  • $v_k$ 观测噪声，服从高斯分布 $v_k\sim N(0,R)$，$R$ 即下文测量噪声

• 卡尔曼滤波的两个假设

  • 信息过程的足够精确的模型，是由白噪声所激发的线性( 也可以是时变的) 动态系统;
  • 每次的测量信号都包含着附加的白噪声分量 。当满足以上假设时，可以应用卡尔曼滤波算法。

2. 5个公式

2.1 预测

根据上一时刻( $k-1$ 时刻) 的后验估计值来估计当前时刻( $k$ 时刻) 的状态，得到 $k$​ 时刻的先验估计值;
$${\hat{x}}_{\overline{k}}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$

• ${\hat{x}}_{\overline{k}}$：$k$ 时刻的先验状态估计值，即根据上一时刻（$k-1$时刻）的最优估计预测的 $k$ 时刻的结果。
• ${\hat{x}}_{k-1}$ 表示 $k-1$ 时刻的后验状态估计值，也叫最优估计，是滤波的结果之一。
• $A$：状态转移矩阵，实际上是对目标状态转换的一种猜想模型。
• $u_{k-1}$：控制向量
• $B$：是将控制向量$u_{k-1}$转换为状态的矩阵

$$P_{\overline{k}}=A{P}_{k-1}{A}^T+Q$$

• $P_{\overline{k}}$：$k$ 时刻的先验估计协方差（${\hat{x}}_{\overline{k}}$ 的协方差)
• $A$：状态转移矩阵，实际上是对目标状态转换的一种猜想模型。
• $P_{k-1}$： $k-1$ 时刻后验估计协方差（即${\hat{x}}_{k-1}$ 的协方差，表示状态的不确定度），是滤波的结果之一。
• $Q$: 过程激励噪声协方差（系统过程的协方差）。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号， 所以 $Q$ 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声。状态转移协方差矩阵

2.2 更新

使用当前时刻的测量值来更正预测阶段先验估计值，得到当前时刻的后验估计值。
$$K_k=\frac{P_{\overline{k}}{H}^T}{H{P}_{\overline{k}}{H}^T+R}$$

• $K_k$：滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。
• $P_{\overline{k}}$：$k$ 时刻的先验估计协方差（${\hat{x}}_{\overline{k}}$ 的协方差)
• $H$：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
• $R$：测量噪声协方差。滤波器实际实现时，测量噪声协方差 R一般可以观测得到，是滤波器的已知条件。

$${\hat{x}}_k={\hat{x}}_{\overline{k}}+K_k(z_k-H{\hat{x}}_{\overline{k}})$$

• ${\hat{x}}_k$ 表示 $k$ 时刻的后验状态估计值，也叫最优估计，是滤波的结果之一。
• ${\hat{x}}_{\overline{k}}$：$k$ 时刻的先验状态估计值，即根据上一时刻（$k-1$时刻）的最优估计预测的 $k$ 时刻的结果。
• $K_k$：滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。
• $z_k$：测量值（观测值），是滤波的输入。
• $H$：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系。
• $z_k-H{\hat{x}}_{\overline{k}}$：实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。

$$P_k=(I-K_{k}H)P_{\overline{k}}$$

• $P_k$： $k$ 时刻后验估计协方差（即${\hat{x}}_k$ 的协方差，表示状态的不确定度），是滤波的结果之一。

• $I$：单位矩阵

• $K_k$：卡尔曼增益，或卡尔曼系数。

• $H$：是状态变量到测量（观测）的转换矩阵。

• $P_{\overline{k}}$：$k$ 时刻的先验估计协方差（${\hat{x}}_{\overline{k}}$ 的协方差)

3. 代码实现

prediction
$$\begin{gathered} x^{\prime }=Fx+u \\ {P}^{\prime }=FP{F}^T+Q \end{gathered}$$
update
$$\begin{gathered} y=z-H{x}^{\prime } \\ S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{-1} \\ x=x^{\prime }+Ky \\ P=(I-KH)P^{\prime } \end{gathered}$$
code