kalman filter 卡尔曼滤波器公式推导

Kalman filter

一个线性时不变系统如下：

$$x_{k+1} = Ax_k+Bu_k+w_k\\ y_k = Cx_k+v_k$$
其中， $w_k$是process noise, $w_k\in\mathbb{R}^n$, $w_k\sim \mathcal{N}(0, Q)$, $v_k$是mearsurement , $v_k\sim\mathbb{R}^{m}$ $v_k\in\mathcal{N}(0,R)$, $A,B,C$分别为系统参数， $x_{k+1}$代表 $k+1$ 时刻状态变量的值， $y_k$表示 $k$ 时刻测量值。

以下内容参考维基百科：
卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

卡尔曼滤波器的状态由以下两个变量表示：
$\hat x_{k|k}$，在时刻k的状态的估计；
${\textbf {P}}_{k|k}$，后验估计误差协方差矩阵，度量估计值的精确程度。

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测阶段：
1. 预测状态

$$\hat x_{k+1|k} = A\hat x_{k|k}+Bu_k$$
2. 预测估计协方差矩阵
$$P_{k+1|k} = AP_{k}A^T+Q$$

更新阶段：

$$K_k = P_{k|k-1}C^T(CP^TC+R)^{-1}$$
未完，有时间继续写。