卡尔曼(kalman)滤波器是一种线性滤波器,主要涉及到5个公式,包括预测部分(Predict)和更新部分(Update)。卡尔曼滤波器中加入了不确定性的估计和更新,使得其比较适合实际应用。由于项目需要对采集的时间序列数据进行处理,尝试使用卡尔曼滤波器进行数据的滤波和预测等操作。具体的卡尔曼滤波的原理和基础知识可以参考卡尔曼滤波(Kalman Filter)(1)。
卡尔曼滤波中噪声的引入使用了协方差矩阵,协方差矩阵定义为:若n维随机变量
(x1,x2,...,xn)
的二阶混合中心矩
σij=cov(xi,xj)=E[(xi−E(xi))(xj−E(xj))]
都存在,则
称为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
:卡尔曼系数,也叫滤波增益矩阵
H
:观测矩阵
(4)
xt^=xt^−+Kt(yt−Hxt^−)
说明:
此公式是状态修正公式
其中
yt−Hxt^−
表示实际观测值与预估值之间的残差
(5)
Σt=(I−KtH)Σ−t
说明:
此公式表明噪声分布的更新
上述5个公式即完成了数据的预测及更新过程。
项目中需要处理的数据是三维空间点数据,所以观测值维数为3,状态维数为6,即三维的位置及每一维的速度,所以状态转移矩阵可以表示为:
控制矩阵设为0,观测矩阵设置为
Q 和
很像之前采用滑动平滑的效果。