Kalman滤波器

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

卡尔曼滤波中噪声的引入使用了协方差矩阵，协方差矩阵定义为：若n维随机变量(x1,x2,...,xn)的二阶混合中心矩
σij=cov(xi,xj)=E[(xi−E(xi))(xj−E(xj))]
都存在，则

Σ=⎡⎣⎢⎢⎢⎢⎢a11a21⋮an1a12a22⋮an2⋯⋯⋱⋯a1na2n⋮ann⎤⎦⎥⎥⎥⎥⎥

称为n维随机变量的协方差矩阵。协方差矩阵是对称矩阵，对角线元素是各个维度的方差。如下是两个协方差矩阵的性质：
cov(AX+a)=Acov(X)AT
cov(AX+a,BTY+b)=Acov(X,Y)B

接下来就是卡尔曼滤波的公式了
预测：
(1) xt^−=Ftxt−1^+Btut
说明：
Ft:状态转移矩阵
Bt:控制矩阵
xt−1^:估计值（非真值）
xt^−:根据上一次的状态推测得到的状态

(2) Σ−t=FΣt−1FT+Q
说明：
Σ−t：每一时刻的不确定性（协方差矩阵），噪声服从Noise∼Gaussian(0,σ)
Q：预测模型本身的噪声（协方差矩阵）

更新：
(3) Kt=Σ−tHT(HΣ−tHT+R)−1
说明：
Kt：卡尔曼系数，也叫滤波增益矩阵
H：观测矩阵
R：观测中的不确定性（协方差矩阵）

(4) xt^=xt^−+Kt(yt−Hxt^−)
说明：
此公式是状态修正公式
其中yt−Hxt^−表示实际观测值与预估值之间的残差

(5) Σt=(I−KtH)Σ−t
说明：
此公式表明噪声分布的更新

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

F=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢100000010000001000100100010010001001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥

控制矩阵设为0，观测矩阵设置为

H=⎡⎣⎢100010001000000000⎤⎦⎥

Q和R简单的设置为对角线元素都相等。三维空间点滤波效果如下：

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