卡尔曼滤波器简介
卡尔曼滤波器是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法,由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。卡尔曼滤波器的解是递归计算的,可以不加修饰地应用于平稳和非平稳环境,状态的每一次更新估计都是由前一次估计和新的输入数据计算获得,因此只需存储前一次估值,所以在计算上更加方便有效。
卡尔曼滤波器假设一个系统的输出是经过线性过程得到的,这个过程可以用两个方程表示,即过程方程和测量方程。
过程方程如下式:
其中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)。
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('幅度');
运行结果
从结果中可以发现,卡尔曼滤波器在很大程度上减小了信号中的噪声。