matlab编程进行卡尔曼滤波的简单例子

1、matlab编程进行卡尔曼滤波的简单例子

clear
N=50;
x(1)=0; %理论速度初值v(1)=0
ut=-270; %加速度值 
F=1; %状态转移矩阵
B=0.01; %控制矩阵 步长值
H=1; %观测矩阵
v=randn(1,N)*5;   %观测噪声 均值=0，方差=5
R=[5];%观测噪声协方差矩阵
w=randn(1,N)*1;%预测噪声，均值为0，方差=1
Q=[1];    %预测模型协方差矩阵

for t=2:N
    x(t)=F*x(t-1)+B*ut; %状态方程 x1(t)代表xt-   ，理论计算速度
end
for i=1:N
    x_noise(i)=x(i)+w(i);%带噪声的速度，模拟实际的速度
    Z(i)=H*x_noise(i)+v(i); %观测值计算,模拟实际的测量速度  
end
%%%%%%以上是仿真获得真实速度x_noise及测量值Z%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%以下为卡尔曼滤波程序，从第2点开始滤波，X（2：30）为滤波后的速度：
X(1)=20; %初始估计速度值
p(1)=[10]; %初始估计协方差矩阵
for t=2:N
    X(t)=F*X(t-1)+B*ut;%预测
    p1(t)=F*p(t-1)*F+Q; %状态转移协方差矩阵 p1(t)代表pt-  
    K(t)=p1(t)*H'/(H*p1(t)*H'+R); %卡尔曼增益计算
    X(t)=X(t)+K(t)*(Z(t)-H*X(t)); %最佳估计值计算
    p(t)=(1-K(t)*H)*p1(t); %协方差矩阵更新
end

figure();
%画出温度计的测量值
plot(Z,'k+'); 
hold on;
%画出最优估计值
plot(X,'r-') 
hold on;
%画出实际值
plot(x_noise,'g-') 
hold on;
%画出纯理论计算值
plot(x,'b-') 
hold on;
xlabel('时间(s)','fontsize',10.5)
ylabel('速度(deg/s)','fontsize',10.5)
set(gcf,'Position',[400,100,425,318]);
legend('纯观测值','卡尔曼最优估计值','实际值','纯理论计算值');
hold off;
figure;
plot(p);
xlabel('最优估计方差','fontsize',10.5)

 

2、根据卡尔曼滤波的原理，当系统状态静止，即ut=0;w=0;Q=0时，如果设置初始状态估计值为测量的第一个值即X（1）=Z(1)，且初始估计方差为测量的方差，即P=R，卡尔曼滤波从第二个值开始进行，则卡尔曼滤波的结果相当于测量值的均值及方差，如下图：

程序如下：

clear
N=50;
x(1)=0; %理论速度初值v(1)=0
ut=0;%-270; %加速度值 
F=1; %状态转移矩阵
B=0.01; %控制矩阵 步长值
H=1; %观测矩阵
v=randn(1,N)*5;   %观测噪声 均值=0，方差=5
R=[5];%观测噪声协方差矩阵
w=randn(1,N)*0;%预测噪声，均值为0，方差=1
Q=[0];    %预测模型协方差矩阵

for t=2:N
    x(t)=F*x(t-1)+B*ut; %状态方程 x1(t)代表xt-   ，理论计算速度
end
for i=1:N
    x_noise(i)=x(i)+w(i);%带噪声的速度，模拟实际的速度
    Z(i)=H*x_noise(i)+v(i); %观测值计算,模拟实际的测量速度  
end
%%%%%%以上是仿真获得真实速度x_noise及测量值Z%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%以下为卡尔曼滤波程序，从第2点开始滤波，X（2：30）为滤波后的速度：
X(1)=Z(1); %初始估计速度值
p(1)=[5]; %初始估计协方差矩阵
for t=2:N
    X(t)=F*X(t-1)+B*ut;%预测
    p1(t)=F*p(t-1)*F+Q; %状态转移协方差矩阵 p1(t)代表pt-  
    K(t)=p1(t)*H'/(H*p1(t)*H'+R); %卡尔曼增益计算
    X(t)=X(t)+K(t)*(Z(t)-H*X(t)); %最佳估计值计算
    p(t)=(1-K(t)*H)*p1(t); %协方差矩阵更新
end

figure();
%画出温度计的测量值
plot(Z,'k+'); 
hold on;
%画出最优估计值
plot(X,'r-') 
hold on;
%画出实际值
plot(x_noise,'g-') 
hold on;
%画出纯理论计算值
plot(x,'b-') 
hold on;
xlabel('时间(s)','fontsize',10.5)
ylabel('速度(deg/s)','fontsize',10.5)
set(gcf,'Position',[400,100,425,318]);
legend('纯观测值','卡尔曼最优估计值','实际值','纯理论计算值');
hold off;
figure;
plot(p);
xlabel('最优估计方差','fontsize',10.5)