Kalman滤波器

卡尔曼(kalman)滤波器是一种线性滤波器，主要涉及到5个公式，包括预测部分(Predict)和更新部分(Update)。卡尔曼滤波器中加入了不确定性的估计和更新，使得其比较适合实际应用。由于项目需要对采集的时间序列数据进行处理，尝试使用卡尔曼滤波器进行数据的滤波和预测等操作。具体的卡尔曼滤波的原理和基础知识可以参考卡尔曼滤波（Kalman Filter）（1）。

卡尔曼滤波中噪声的引入使用了协方差矩阵，协方差矩阵定义为：若n维随机变量$(x_1,x_2,...,x_n)$的二阶混合中心矩
$\sigma_{ij} = cov(x_i,x_j)=E[(x_i-E(x_i))(x_j-E(x_j))]$
都存在，则

$$\Sigma=\begin{bmatrix} {a_{11}}&{a_{12}}&{\cdots}&{a_{1n}}\\ {a_{21}}&{a_{22}}&{\cdots}&{a_{2n}}\\ {\vdots}&{\vdots}&{\ddots}&{\vdots}\\ {a_{n1}}&{a_{n2}}&{\cdots}&{a_{nn}}\\ \end{bmatrix}$$
称为n维随机变量的协方差矩阵。协方差矩阵是对称矩阵，对角线元素是各个维度的方差。如下是两个协方差矩阵的性质：
$cov(AX+a)=Acov(X)A^T$
$cov(AX+a,B^TY+b)=Acov(X,Y)B$

接下来就是卡尔曼滤波的公式了
预测：
(1) $\hat{x_t}^-=F_t\hat{x_{t-1}}+B_tu_t$
说明：
$F_t$:状态转移矩阵
$B_t$:控制矩阵
$\hat{x_{t-1}}$:估计值（非真值）
$\hat{x_t}^-$:根据上一次的状态推测得到的状态

(2) $\Sigma_t^-=F\Sigma_{t-1}F^T+Q$
说明：
$\Sigma_t^-$：每一时刻的不确定性（协方差矩阵），噪声服从$Noise\sim Gaussian(0,\sigma)$
$Q$：预测模型本身的噪声（协方差矩阵）

更新：
(3) $K_t=\Sigma_t^-H^T(H\Sigma_t^-H^T+R)^{-1}$
说明：
$K_t$：卡尔曼系数，也叫滤波增益矩阵
$H$：观测矩阵
$R$：观测中的不确定性（协方差矩阵）

(4) $\hat{x_t}=\hat{x_t}^-+K_t(y_t-H\hat{x_t}^-)$
说明：
此公式是状态修正公式
其中$y_t-H\hat{x_t}^-$表示实际观测值与预估值之间的残差

(5) $\Sigma_t=(I-K_tH)\Sigma_t^-$
说明：
此公式表明噪声分布的更新

上述5个公式即完成了数据的预测及更新过程。
项目中需要处理的数据是三维空间点数据，所以观测值维数为3，状态维数为6，即三维的位置及每一维的速度，所以状态转移矩阵可以表示为：

$$F=\begin{bmatrix} {1}&{0}&{0}&{1}&{0}&{0}\\ {0}&{1}&{0}&{0}&{1}&{0}\\ {0}&{0}&{1}&{0}&{0}&{1}\\ {0}&{0}&{0}&{1}&{0}&{0}\\ {0}&{0}&{0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{0}&{0}&{1}\\ \end{bmatrix}$$
控制矩阵设为0，观测矩阵设置为
$$H=\begin{bmatrix} {1}&{0}&{0}&{0}&{0}&{0}\\ {0}&{1}&{0}&{0}&{0}&{0}\\ {0}&{0}&{1}&{0}&{0}&{0}\\ \end{bmatrix}$$
$Q$和$R$简单的设置为对角线元素都相等。三维空间点滤波效果如下：

很像之前采用滑动平滑的效果。