交互式多模型算法卡尔曼滤波仿真

交互式多模型算法卡尔曼滤波仿真
1 模型建立
分别以加速度u=0、1、2代表三个不同的运动模型
状态方程x(k+1)=a*x(k)+b*w(k)+d*u
观察方程z(k)=c*x(k)+v(k)
其中，a=[1 dt;0 1],b=[dt^2/2;dt],d=[dt^2/2;dt],c=[1 0]
2 程序代码
由两个功能函数组成，imm_main用来实现imm 算法，move_model1用来生成仿真数据，初始化运动参数
function imm_main
%交互式多模型算法主程序
%初始化有关参数
move_model %调用运动模型初始化及仿真运动状态生成函数
load movedata %调入有关参数初始值（a b d c u position velocity pmeas dt tg q r x_hat p_var）
p_tran=[0.8 0.1 0.1;0.2 0.7 0.1;0.1 0.2 0.7];%转移概率
p_pri=[0.1;0.6;0.3];%模型先验概率
n=1:2:5; %提取各模型方差矩阵
k=0; %记录仿真步数
like=[0;0;0];%视然函数
x_hat_hat=zeros(2,3);%三模型运动状态重初始化矩阵
u_=zeros(3,3);%混合概率矩阵
c_=[0;0;0];%三模型概率更新系数
%数据保存有关参数初始化
phat=[];%保存位置估值
vhat=[];%保存速度估值
xhat=[0;0];%融合和运动状态
z=0;%量测偏差（一维位置）
pvar=zeros(2,2);%融合后估计方差
for t=0:dt:tg; %dt为为仿真步长；tg为仿真时间长度
k=k+1;%记录仿真步数
ct=0; %三模型概率更新系数
c_max=[0 0 0];%混合概率规范系数
p_var_hat=zeros(2,6);%方差估计重初始化矩阵,
%[x_hat_hat p_var_hat]=model_reinitialization(p_tran,p_pri,x_hat,p_var);%调用重初始化函数，进行混合估计，生成三个模型重初始化后的运动状态、方差
%混合概率计算
for i=1:3
u_(i,:)=p_tran(i,:)*p_pri(i);
end
for i=1:3
c_max=c_max+u_(i,:);
end
for i=1:3
u_(:,i)=u_(:,i)/c_max(i);
end
%各模型状态、方差重初始化
x_hat_hat=x_hat*u_;%运动状态重初始化
for j=1:3
for i=1:3
p_var_hat(:,n(j):n(j)+1)=
p_var_hat(:,n(j):n(j)+1)+(p_var(:,n(i):n(i)+1)+(x_hat(:,i)-x_hat_hat(:,j))*(x_hat(:,i)-x_hat_hat(:,j))')*u_(i,j);%方差混合估计
end
end
%各模型进行依次次kalman滤波
for i=1:3 %各模型进行依次次kalman滤波
% 模型条件滤波
x_hat(:,i)=a*x_hat_hat(:,i)+d*u(i);%一步状态预测
p_var(:,n(i):n(i)+1)=a*p_var_hat(:,n(i):n(i)+1)*a'+b*q*b';%一步状态预测方差
z=pmeas(k)-c*x_hat(:,i);%量测误差估计
s=c*p_var(:,n(i):n(i)+1)*c'+r;%量测方差
k_add=p_var(:,n(i):n(i)+1)*c'*inv(s);%kalman增益
x_hat(:,i)=x_hat(:,i)+k_add*z;
p_var(:,n(i):n(i)+1)=p_var(:,n(i):n(i)+1)-k_add*s*k_add';
%计算与当前模型匹配的视然函数
like(i)=1/sqrt(2*pi*s)*exp(-1/2*z^2/s);
end
% 模型概率更新
c_=p_tran*p_pri;
for i=1:3
c_(i)=c_(i)*like(i);
ct=ct+c_(i);
end
p_pri=c_/ct;%模型先验概率更新
xhat=x_hat*p_pri;%运动状态估计融合
for i=1:3 %方差融合
pvar=pvar+(p_var(:,n(i):n(i)+1)+(xhat-x_hat(:,i))*(xhat-x_hat(:,i))');
end
phat=[phat;xhat(1)];%位置估计值保存
vhat=[vhat;xhat(2)];%速度估计值保存
end
% 图形输出
t=0:dt:tg;
subplot(3,2,1);
%ylabel(‘Position (m)’);
plot(t,position);
grid;
title('真位置');
subplot(3,2,2);
plot(t,velocity);
grid;
title('真速度');
subplot(3,2,3);
plot(t,pmeas);
grid;
title('位置量测值');
subplot(3,2,4);
plot(t,vhat);
grid;
title('速度估计值');
subplot(3,2,5);
plot(t,phat);
grid;
title('位置估计值');
subplot(3,2,6);
plot(t,position-phat,t,velocity-vhat);
grid;
title('位置（蓝）、速度估计误差（绿）');
function move_model1
% 参数初始化
dt=0.1;%仿真步长（秒）
tg=200;%仿真持续时间（秒）
a=[1 dt;0 1];%状态转移矩阵
b=[dt^2/2;dt];%激励输入矩阵
d=[dt^2/2;dt];%机动加速度系数矩阵
c=[1 0];%量测矩阵
x=[0;0];%初始化状态矢量
x_hat=[x x x];%初始化状态估计矩阵（三个模型状态矢量综合考虑）
q=0.04;%过程随机噪声方差
r=36;%量测随机噪声方差
p_var=[b*q*b' b*q*b' b*q*b'];%初始化方差矩阵（三个模型状态矢量综合考虑）% 数据保存数组初始化
position=[];%真实位置
pmeas=[];%位置量测值
velocity=[];%真实速度
u=0:1:2;
%k=0:dt:tg
% 生成仿真模拟数据
for i=0:dt:tg
w=0.2*randn;%过程随机噪声，均值为0，方差为0.04
v=6*randn;%量测随机噪声，均值为0，方差为36
if i<=30
x=a*x+b*w+d*u(2);
elseif i>30&i<100
x=a*x+b*w+d*u(1);
elseif i>100&i<150
x=a*x+b*w+d*u(3);
elseif i>150&i<180
x=a*x+b*w+d*u(1);
else
x=a*x+b*w+d*u(3);
end
y=c*x+v;%量测值计算
position=[position;x(1)];%真实位置
pmeas=[pmeas;y];%位置量测值
velocity=[velocity;x(2)];%真实速度
end
save movedata a b d c u position velocity pmeas dt tg q r x_hat p_var
3 运行结果
图一：
机动方式： 0-30秒：u=1； 30-100秒：u=0； 100-150秒：u=2； 150-180秒：u0； 180-200秒：u=2 过程噪声方差：0.04 量测噪声方差：36
050100150200
05000
10000
15000真位置
050100150200
0100
200
真速度
50
100
150
200
-1012
4
位置量测值0
50
100
150
200
100
200速度估计值
50
100
150
200
05000
1000015000位置估计值
050100150200
-10
10
位置（蓝）、速度估计误差（绿）
图一
图二：
机动方式： 0-30秒：u=2； 30-100秒：u=0； 100-150秒：u=1； 150-180秒：u=0； 180-200秒：u=2 过程噪声方差：4 量测噪声方差：100
050100150200
05000
10000
15000真位置
050100150200
0100
200真速度
50
100
150
200
-1012
4
位置量测值0
50
100
150
200
100
200速度估计值
50
100
150
200
05000
1000015000位置估计值
050100150200
-10
10
位置（蓝）、速度估计误差（绿）
图二。