卡尔曼滤波和机器学习 卡尔曼滤波 程序

卡尔曼滤波器

• 卡尔曼滤波器简介
• MATLAB程序
• 运行结果

卡尔曼滤波器简介

卡尔曼滤波器是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波器的解是递归计算的，可以不加修饰地应用于平稳和非平稳环境，状态的每一次更新估计都是由前一次估计和新的输入数据计算获得，因此只需存储前一次估值，所以在计算上更加方便有效。

卡尔曼滤波器假设一个系统的输出是经过线性过程得到的，这个过程可以用两个方程表示，即过程方程和测量方程。

过程方程如下式：

其中x(n)向量是状态向量，是需要进行估计、求解的量；F(n+1,n)是状态转移矩阵；v₁(n)向量表示过程噪声，可建模为零均值的白噪声过程，其相关矩阵使用Q₁(n)表示。

测量方程如下式：

其中y(n)向量为系统输出，是观测到的量；C(n)为测量矩阵；v₂(n)向量表示测量噪声，可建模为与v₁(n)向量互不相关的零均值白噪声过程，其相关矩阵使用Q₂(n)表示。

有了这两个方程后，要解决的问题就是通过观测量y(n)向量在给定Q₁(n)和Q₂(n)的条件下对x(n)向量进行估计。卡尔曼滤波器的递推方程如下：

其中G(n)为卡尔曼增益矩阵，a(n)向量为新息过程向量，x(n|Y_n-1)向量表示在Y_n-1观测空间下对于状态x(n)向量的估计，K(n)为状态误差的相关矩阵，K(n,n-1)为x(n|Y_n-1)向量中误差的相关矩阵。

卡尔曼滤波器递推的初始条件为：

其中E[a]表示对a求数学期望，K₀为常数。

在调整滤波器时可调的参数是过程噪声的自相关矩阵Q₁(n)和测量噪声的自相关矩阵Q₂(n)。

MATLAB程序

%============== Kalman filtering ================
% 根据《自适应滤波器原理》第424页
% 过程方程：x(n+1) = F(n+1,n)x(n) + v1(n);
% 测量方程：y(n)   = C(n)x(n)     + v2(n);
clc
clear
close all
 
%=============== generate data ====================
dotnumber=200;
 
Fs=100;
Fsignal=5;
t=(0:dotnumber-1)/Fs;
xn=cos(2*pi*Fsignal*t);
noise=0.6*wgn(1,dotnumber,-10);% 正弦波加白噪声
 
 
data = xn + noise;
 
%=============== design Kalman filtering ====================
% 相关参数
F = 1;          % 状态转移矩阵
C = 1;          % 测量矩阵
Q1 = 0.01;      % 过程噪声：减小->平滑、赋值下降、相位偏离    增大->跟随
Q2 = 0.1;       % 测量噪声：减小->跟随                        增大->收敛慢、平滑、赋值下降、相位偏离
 
X = data(1);    % 系统初始状态
K = 1;          % 卡尔曼增益初始状态
 
alpha = 0;      % 新息
Xcap = data(1); % 系统状态估值
data_cap = zeros(1,dotnumber);% 用来存储状态预测值
data_cap(1)=Xcap;
 
for i = 2:dotnumber
    X = data(i);% 更新输入数据
    
    G = F*K*C/(C*K*C+Q2);   % 更新卡尔曼增益
    alpha = X-C*Xcap;       % 更新新息
    Xcap = F*Xcap+G*alpha;  % 更新状态估计值
    K = K-F*G*C*K;
    K = F*K*F+Q1;           % 更新状态误差相关矩阵
    
    data_cap(i) = Xcap;
end
 
figure;
subplot(3,1,1);
plot(t,xn);
axis([0 inf -2 2]);
legend('原始信号');
xlabel('时间/s');ylabel('幅度');
subplot(3,1,2);
plot(t,data);
axis([0 inf -2 2]);
legend('加噪声信号');
xlabel('时间/s');ylabel('幅度');
subplot(3,1,3);
plot(t,data_cap);
axis([0 inf -2 2]);
legend('滤波后信号');
xlabel('时间/s');ylabel('幅度');

运行结果

从结果中可以发现，卡尔曼滤波器在很大程度上减小了信号中的噪声。