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)

 

matlab编程进行卡尔曼滤波的简单例子_第1张图片

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

matlab编程进行卡尔曼滤波的简单例子_第2张图片

程序如下:

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)

 

你可能感兴趣的:(matlab,数学)