卡尔曼滤波器是一种利用线性系統状态方程通过系统输入输出观测数据,对系统状态进行最优估计的算法由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程卡尔曼滤波器的解是递归计算的,可以不加修饰地应用于平稳和非平稳环境状态的每一次更新估计都是由前┅次估计和新的输入数据计算获得,因此只需存储前一次估值所以在计算上更加方便有效。
卡尔曼滤波器假设一个系统的输出是经过线性过程得到的这个过程可以用两个方程表示,即过程方程和测量方程
其中x(n)向量是状态向量,是需要进行估计、求解的量;F(n+1,n)是状态转移矩阵;v1(n)向量表示过程噪声可建模为零均值的白噪声过程,其相关矩阵使用Q1(n)表示
其中y(n)向量为系统输出,是观测到的量;C(n)为测量矩阵;v2(n)向量表示测量噪声可建模为与v1(n)向量互不相关的零均值白噪声过程,其相关矩阵使用Q2(n)表示
有了这两个方程后,要解决的问题就是通过观测量y(n)向量在给定Q1(n)和Q2(n)的条件下对x(n)向量进行估计卡尔曼滤波器的递推方程如下:
其中G(n)为卡尔曼增益矩阵,a(n)向量为新息过程向量x(n|Yn-1)向量表示在Yn-1观測空间下对于状态x(n)向量的估计,K(n)为状态误差的相关矩阵K(n,n-1)为x(n|Yn-1)向量中误差的相关矩阵。
卡尔曼滤波器递推的初始条件为:
其中E[a]表示对a求数学期望K0为常数。
在调整滤波器时可调的参数是过程噪声的自相关矩阵Q1(n)和测量噪声的自相关矩阵Q2(n)
% 根据《自适应滤波器原理》第424页 Q2 = 0.1; % 测量噪声:减小->跟随 增大->收敛慢、平滑、赋值下降、相位偏离
从结果中可以发现,卡尔曼滤波器在很大程度上减小了信号中的噪声