卡尔曼滤波

卡尔曼滤波（kalman filter）

算法模型

卡尔曼滤波器用于估计离散时间过程的状态变量$x\in R^n$ ，这个离散时间过程由以下离散随机差分方程描述：
$$x_k=A{x}_{k-1}+B{u}_k+w_k\text{\hspace{1em}(1)}$$
其中 $x_k$ 表示 $k$​ 时刻的真实状态值，$u_k$ 表示 $k$ 时刻的控制值，$w_k$ 为过程噪声。

定义观测变量$z\in R^m$，得到观测方程：
$$z_k=H{x}_k+v_k\text{\hspace{1em}(2)}$$
随机信号 $w_k$ 和 $v_k$ 分别表示过程噪声和观测噪声。假设它们为相互独立，正态分布的白噪声：
$$\begin{gathered} p(w)\sim N(0,Q) \\ p(v)\sim N(0,R)\text{\hspace{1em}(3)} \end{gathered}$$

算法过程

1. 预测阶段：

$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}\text{\hspace{1em}(4)}$$

其中 ${\hat{x}}_k^{-}\in R^n$​ (${}^{-}$​ 表示先验，^表示估计)表示在已知第 $k$​ 时刻之前状态的情况下，第 $k$​ 时刻的先验状态估计，$A$​ 表示$n\times n$​ 阶状态转移矩阵，$B$​ 表示$n\times 1$​ 阶控制转移矩阵。${\hat{x}}_{k-1}\in R^n$​ 表示为已知道观测变量时第 $k-1$​ 时刻的后验状态估计。$u_{k-1}$​ 表示$k-1$​ 时刻的控制量。
$$P_k^{-}=A{P}_{k-1}{A}^T+Q\text{\hspace{1em}(5)}$$
其中$P_k^{-}$​ 表示先验估计误差（状态预测值与真实值的误差）协方差矩阵，$P_{k-1}$​ 表示后验估计误差（观测结果与真实值的误差）协方差矩阵，$Q$​ 表示为过程噪声协方差矩阵。

2. 矫正阶段：

$$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}\text{\hspace{1em}(6)}$$

其中 $H$​ 表示状态变量 $x_k$ 对观测变量 $z_k$ 的增益矩阵， $R$​ 表示观测噪声协方差矩阵。上述式​ 表示计算卡尔曼增益（kalman gain）。
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\text{\hspace{1em}(7)}$$

上述式（7）表示由第 $k$ 时刻的先验估计值 ${\hat{x}}_k^{-}$、实际观测值 $z_k$、卡尔曼增益 $K_k$ 来计算得到第 $k$ 时刻的后验估计值 ${\hat{x}}_k$​ 。
$$P_k=(I-K_{k}H)P_k^{-}\text{\hspace{1em}(8)}$$
其中 $I$​​​ 表示单位矩阵，上述式（8）表示由第 $k$​​ 时刻的先验估计误差协方差矩阵 $P_k^{-}$​​​ 、增益矩阵 $H$​​、卡尔曼增益 $K_k$​​ 来计算得到第 $k$​​ 时刻的后验估计误差协方差矩阵 $P_k$​​​ 。为下一次的迭代做好准备。

参数解释

$A$​ 表示 $n\times n$​ 阶状态转移矩阵

$B$​​​ 表示 $n\times 1$​​​​ 阶控制转移矩阵

$R$ 表示 $m\times m$ 阶观测噪声协方差矩阵

$H$​​​ 表示状态变量 $x_k$​​​ 对观测变量 $z_k$​​​ 的 $m\times n$​ 阶增益矩阵

$Q$ 表示为过程噪声协方差矩阵，$n\times n$ 阶

${\hat{x}}_k^{-}\in R^n$ (${}^{-}$ 表示先验，^表示估计)表示在已知第 $k$ 时刻之前状态的情况下，第 $k$​ 时刻的先验状态估计

$u_{k-1}$​ 表示$k-1$​ 时刻的控制量

$P_k^{-}$​ 表示第 $k$​​ 时刻先验估计误差（状态预测值与真实值的误差）协方差矩阵，$n\times n$ 阶

$P_k$​​​ 表示第 $k$​​​​ 时刻的后验估计误差协方差矩阵，$n\times n$ 阶

$K_k$​​​​ 表示第 $k$​​​​ 时刻的卡尔曼增益，$n\times 1$​ 阶