kalman滤波器算法的详细介绍

kalman滤波器

一．什么是卡尔曼滤波器

卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯, 我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

二.卡尔曼滤波器算法的介绍

以下是卡尔曼滤波器核心的5个式子。

X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)由（K-1）时刻的最优值X(k-1|k-1)得出K 时刻系统的预测值X(k|k-1)，U(k)为控制量。

P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)由（K-1）时刻最优值的偏差P(k-1|k-1) 及系统本身偏差Q得到K时刻系统预测值的偏差P(k|k-1)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ………(3)由K时刻系统预测值X(k|k-1)及测量值Z（K）还有卡尔曼增益Kg(k)得到K时刻系统的最优值估计，即X(k|k) Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)由K时刻系统预测值的偏差P(k|k-1)及预测值误差R得到卡尔曼增益Kg(k)

P(k|k)=（I-Kg(k) H）P(k|k-1) …………(5)由K时刻系统预测值的偏差P(k|k-1)及卡尔曼增益得到K时刻最优值X(k|k)的估计偏差P(k|k)

以上介绍是针对单值卡尔曼滤波的预测，若为多值，则相应Q,R等应为矩阵形式。三．卡尔曼滤波的Matlab仿真

源程序如下：

clear

clc;

N=600;%采样点的个数

CON=25;%室内温度的理论值

x=zeros(1,N);%用来记录温度的最优化估算值

y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声

x(1)=20;%为x(k)赋初值

p(1)=2;%x(1)对应的协方差

Q=cov(randn(1,N));%过程噪声的协方差

R=cov(randn(1,N));%测量噪声的协方差

for k=2:N%循环里面是卡尔曼滤波的具体过程

x(k)=x(k-1);

p(k)=p(k-1)+Q;

Kg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益

x(k)=x(k)+Kg(k)*(y(k)-x(k));

p(k)=(1-Kg(k))*p(k);

end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%这个模块起到平滑滤波作用

Filter_Width=10;%滤波器带宽

Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值

for i=Filter_Width+1:N

Temp_Sum=0;

for j=i-Filter_Width:(i-1)

Temp_Sum=x(j)+Temp_Sum;

end

Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值

end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

t=1:N;

figure('Name','Kalman Filter Simulation','NumberTitle','off');

expected_Value=zeros(1,N);

for i=1:N

expected_Value(i)=CON;

end

plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，

legend('expected','measure','estimate','smooth result'); %经过kalman 滤波后的最优化估算值，平滑滤波后的输出值

xlabel('sample time');

ylabel('temperature');

title('Kalman Filter Simulation');