Question

求IMM算法的matlab仿真代码

Answer 1

function imm_extend2() %交互式多模型 初步改进
clear all;
close all;
timenum=75;
t=1;
transfer_matric=[0.98 0.02; %马尔科夫转移矩阵 矩阵为j行i列
0.02 0.98];
u_10=0.5; %目标1在模型i在k-1时刻的概率
u_20=0.5; %目标1在模型i在k-1时刻的概率
u=[u_10 u_20];
Q1=100; %目标1的状态噪声协方差阵
Q2=4000; %目标1的状态噪声协方差阵
R=10000; %观测噪声协方差阵
%R2=700;
x00_1=[0 0 0]'; %模型1估计的初始值
x00_2=[0 0 0]'; %模型2估计的初始值
p00_1=[100 0 0;
0 10 0;
0 0 0];
p00_2=[100 0 0;
0 10 0;
0 0 10];
dis=1000; %位移
vel=450; %速度
acc=50; %加速度
xx01=[dis vel 0]'; %状态的初始值
xx02=[dis vel acc]'; %状态的初始值
%xx2=0;
f1=[1 t 0;
0 1 0;
0 0 0]; %状态转移阵
f2=[1 t 0.5*t^2;
0 1 t;
0 0 1];
l1=[0.5*t^2 t 0]'; %状态干扰阵
l2=[0.5*t^2 t 1]';
h=[1 0 0]; %观测转移阵
I=[1 0 0;
0 1 0;
0 0 1];
num=1;
XX1=zeros(3,25);
XX2=XX1;
XX3=XX1;
while num<=timenum
if num<=25
xx1=f1*xx01+l1*sqrt(Q1); %目标的状态值
XX1(:,num)=xx1;
z(num)=h*xx1+sqrt(R)*randn(1); %目标的观测值
else
if 25<num<=50
xx2=f2*xx02+l2*sqrt(Q2); %目标的状态值
XX2(:,num-25)=xx2;
z(num)=h*xx2+sqrt(R)*randn(1); %目标的观测值
else
if 50<num<=timenum
xx3=f1*xx01+l1*sqrt(Q1); %目标的状态值
XX3(:,num-50)=xx3;
z(num)=h*xx3+sqrt(R)*randn(1); %目标的观测值
end;
end;
end;

%================input alternation==============================
tran_11=transfer_matric(1,1)*u(1);
tran_12=transfer_matric(1,2)*u(1);
tran_21=transfer_matric(2,1)*u(2);
tran_22=transfer_matric(2,2)*u(2);
sum_tran_j1=tran_11+tran_21;
sum_tran_j2=tran_12+tran_22;
sum_tran_j=[sum_tran_j1 sum_tran_j2];
u_mix(1,1)=tran_11/sum_tran_j1;
u_mix(1,2)=tran_12/sum_tran_j2;
u_mix(2,1)=tran_21/sum_tran_j1;
u_mix(2,2)=tran_22/sum_tran_j2;
% for i=1:2
% x00_estimate_j(i)=sum_tran_j(i)*x00(i);
% p00_estimate_j(i)=sum_tran_j(i)*(p00+(x00-x00_estimate_j(i))*(x00-x00_estimate_j(i))');%p00_estimate以行存储
%end;
x00_estimate_j(:,1)=x00_1*u_mix(1,1)+x00_2*u_mix(2,1);
x00_estimate_j(:,2)=x00_1*u_mix(1,2)+x00_2*u_mix(2,2);
p00_estimate_j1=(p00_1+(x00_1-x00_estimate_j(:,1))*(x00_1-x00_estimate_j(:,1))')*u_mix(1,1)+...
(p00_2+(x00_2-x00_estimate_j(:,1))*(x00_2-x00_estimate_j(:,1))')*u_mix(2,1);
p00_estimate_j2=(p00_1+(x00_1-x00_estimate_j(:,2))*(x00_1-x00_estimate_j(:,2))')*u_mix(1,2)+...
(p00_2+(x00_2-x00_estimate_j(:,2))*(x00_2-x00_estimate_j(:,2))')*u_mix(2,2);
%==================filter calculate=================================
%================model 1===================================
x1_pre=f1*x00_estimate_j(:,1);
p1_pre=f1*p00_estimate_j1*f1'+l1*Q1*l1';
s1=h*p1_pre*h'+R;
k_gain1=p1_pre*h'*inv(s1);
p1_estimate=(I-k_gain1*h)*p1_pre;
v1=z(num)-h*x1_pre;
x1_estimate=x1_pre+k_gain1*v1;
X1_estimate(:,num)=x1_estimate;
%================model 2===================================
x2_pre=f2*x00_estimate_j(:,2);
p2_pre=f2*p00_estimate_j2*f2'+l2*Q2*l2';
s2=h*p2_pre*h'+R;
k_gain2=p2_pre*h'*inv(s2);
p2_estimate=(I-k_gain2*h)*p2_pre;
v2=z(num)-h*x2_pre;
x2_estimate=x2_pre+k_gain2*v2;
X2_estimate(:,num)=x2_estimate;
%===============================================================
%===================model probability alternate=================
p_pre=[p1_pre;p2_pre];
x_pre=[x1_pre' x2_pre'];
s=[s1(1) s2(1)];
v=[v1 v2];
for i=1:2
model_fun(i)=(1/sqrt(2*pi*s(i)))*exp(-v(i)*v(i)'/(2*abs(s(i))));
end;
sum_tran=model_fun(1)*sum_tran_j1+model_fun(2)*sum_tran_j2;
u_j1=model_fun(1)*sum_tran_j1/sum_tran;
u_j2=model_fun(2)*sum_tran_j2/sum_tran;
u_j=[u_j1 u_j2];
%==============================================================
%====================output alternation=========================
x_sum=x1_estimate*u_j(1)+x2_estimate*u_j(2)

X_sum(:,num)=x_sum;
p_sum=(p1_estimate+(x_sum-x1_estimate)*(x_sum-x1_estimate)')*u_j1+(p2_estimate+(x_sum-x2_estimate)*(x_sum-x2_estimate)')*u_j2;
P_sum(3*num-2:3*num,1:3)=p_sum;
%x00=x_sum(k);
%x00=[x1_estimate(k) x2_estimate(k)];
x00_1=x1_estimate;
x00_2=x2_estimate;
%p00=[p1_estimate(k) p2_estimate(k)];
p00_1=p1_estimate;
p00_2=p2_estimate;

if (num<=25|25<num<=75)
xx01=xx1;
else if 25<num<=50
xx02=xx2;
%else
% xx01=xx3;
end;
end;

%xx2=xx_2(k);
u=[u_j1 u_j2];
num=num+1;
end;
XX=[XX1 XX2 XX3];
figure;
plot(XX(1,:)','b');
hold on;
plot(X_sum(1,:),'m')
legend('状态','融合输出的估计');
plot(z,'r')

xlabel('采样次数');
ylabel('相应值');
grid;
hold off;
%figure;
%plot(p1_estimate,'b');
%hold on;
%plot(p2_estimate,'m');
%plot(p_sum,'r');
%hold off;
%legend('模型1的协方差','模型2的协方差','融合输出的协方差');
%xlabel('采样次数');
%ylabel('相应值');
%grid;

figure;
plot(XX(2,:)','b');
hold on;
plot(X_sum(2,:)','r');
legend('状态','融合输出的估计');
xlabel('采样次数');
ylabel('相应值');
grid;
hold off;

figure;
plot(XX(3,:),'b');
hold on;
plot(X_sum(3,:)','r');
legend('状态','融合输出的估计');
xlabel('采样次数');
ylabel('相应值');
grid;
hold off;
X_sum
XX

Answer 2

ww.iitk.ac.in/kangal/codes.shtml" target="_blank">http://www.iitk.ac.in/kangal/codes.shtml下载C语言的，matlab的在百度上多的是，如果要自己写，用matlab容易写。