%%
%UNTITLED3 Summary of this function goes here
% 該函式的目的是利用互動多模型演算法對機動目標進行跟蹤
%%%引數意義如下：
%%% MMX0，輸出目標狀態
%%% sigmar，目標位置誤差標準差
%%% Pt，markov模型轉移概率，3*3矩陣
%%% u0，模型先驗概率
%%% T，雷達采樣時間間隔
%%% MX0，目標位置量測值
%%
NN=length(MX0(1,:));
%%%%%%引數初始化%%%%%%%
MMX0=zeros(6,NN);
%%%%利用前三次量測值初始化%%%
MMX0(1,1)=MX0(1,1);
MMX0(1,2)=MX0(1,2);
MMX0(1,3)=MX0(1,3);
MMX0(2,2)=(MX0(1,2)-MX0(1,1))/T;
MMX0(2,3)=(MX0(1,3)-MX0(1,2))/T;
MMX0(3,3)=(MX0(1,3)-2*MX0(1,2)+MX0(1,1))/T;
MMX0(4,1)=MX0(2,1);
MMX0(4,2)=MX0(2,2);
MMX0(4,3)=MX0(2,3);
MMX0(5,2)=(MX0(2,2)-MX0(2,1))/T;
MMX0(5,3)=(MX0(2,3)-MX0(2,2))/T;
MMX0(6,3)=(MX0(2,3)-2*MX0(2,2)+MX0(2,1))/T;
MMXj=[MMX0(:,3),MMX0(:,3),MMX0(:,3)];%%%三個模型的初始狀態
%%%%%%初始化各模型新息協方差%%%%%%%%
R=sigmar^2*eye(2);
P11=R(1,1);
P12=R(1,1)/T;
P13=R(1,1)/T^2;
P22=2*R(1,1)/T^2;
P23=3*R(1,1)/T^3;
P33=6*R(1,1)/T^4;
P44=R(2,2);
P45=R(2,2)/T;
P46=R(2,2)/T^2;
P55=2*R(2,2)/T^2;
P56=3*R(2,2)/T^3;
P66=6*R(2,2)/T^4;
Pj1=[P11 P12 P13 0 0 0;
P12 P22 P23 0 0 0;
P13 P23 P33 0 0 0;
0 0 0 P44 P45 P46;
0 0 0 P45 P55 P56;
0 0 0 P46 P56 P66];
Pj2=Pj1;
Pj3=Pj1;
%%%%%初始化程序噪聲協方差%%%%%%
QQQ=[T^5/20 T^4/8 T^3/6;
T^4/8 T^3/3 T^2/2;
T^3/6 T^2/2 T];
QQ=[QQQ zeros(3);zeros(3) QQQ];
Q1=10*QQ;%%%模型1，q=10
Q2=QQ;%%%模型2，q=1
Q3=0.1*QQ;%%%模型3，q=0.1
F1=[1 T 0.5*T^2;0 1 T;0 0 1];
F=[F1,zeros(3);zeros(3),F1];%%%狀態矩陣
H=[1 0 0 0 0 0;0 0 0 1 0 0 ];%%%測量矩陣
X1=MMXj(:,1);
X2=MMXj(:,2);
X3=MMXj(:,3);%%%迭代初始值
%%
%%%%%%%迭代%%%%%%%
for k=4:NN
%%%%%%%互動作用%%%%%%
Cj=u0*Pt;
for i=1:3
for j=1:3
ukk(i,j)=Pt(i,j)*u0(i)/Cj(j);
end
end
MMXoj=MMXj*ukk;%%%%互動狀態輸出
Poj11=(Pj1+(MMXj(:,1)-MMXoj(:,1))*(MMXj(:,1)-MMXoj(:,1))')*ukk(1,1);
Poj12=(Pj2+(MMXj(:,2)-MMXoj(:,1))*(MMXj(:,2)-MMXoj(:,1))')*ukk(2,1);
Poj13=(Pj3+(MMXj(:,3)-MMXoj(:,1))*(MMXj(:,3)-MMXoj(:,1))')*ukk(3,1);
Poj1=Poj11+Poj12+Poj13;
Poj21=(Pj1+(MMXj(:,1)-MMXoj(:,2))*(MMXj(:,1)-MMXoj(:,2))')*ukk(1,2);
Poj22=(Pj2+(MMXj(:,2)-MMXoj(:,2))*(MMXj(:,2)-MMXoj(:,2))')*ukk(2,2);
Poj23=(Pj3+(MMXj(:,3)-MMXoj(:,2))*(MMXj(:,3)-MMXoj(:,2))')*ukk(3,2);
Poj2=Poj21+Poj22+Poj23;
Poj31=(Pj1+(MMXj(:,1)-MMXoj(:,3))*(MMXj(:,1)-MMXoj(:,3))')*ukk(1,3);
Poj32=(Pj2+(MMXj(:,2)-MMXoj(:,3))*(MMXj(:,2)-MMXoj(:,3))')*ukk(2,3);
Poj33=(Pj3+(MMXj(:,3)-MMXoj(:,3))*(MMXj(:,3)-MMXoj(:,3))')*ukk(3,3);
Poj3=Poj31+Poj32+Poj33;%%%%互動后狀態方差輸出
%%%%%%%%模型修正，卡爾曼濾波%%%%%%%%
XX1=F*X1;
ZZ1=H*XX1;
PP1=F*Poj1*F'+Q1;
SS1=H*PP1*H'+R;
VV1=MX0(:,k)-ZZ1;
WW1=PP1*H'*inv(SS1);
X1=XX1+WW1*VV1;
Pj1=PP1-WW1*SS1*WW1';
MMXj(:,1)=X1;
XX2=F*X2;
ZZ2=H*XX2;
PP2=F*Poj2*F'+Q2;
SS2=H*PP2*H'+R;
VV2=MX0(:,k)-ZZ2;
WW2=PP2*H'*inv(SS2);
X2=XX2+WW2*VV2;
Pj2=PP2-WW2*SS2*WW2';
MMXj(:,2)=X2;
XX3=F*X3;
ZZ3=H*XX3;
PP3=F*Poj3*F'+Q3;
SS3=H*PP3*H'+R;
VV3=MX0(:,k)-ZZ3;
WW3=PP3*H'*inv(SS3);
X3=XX3+WW3*VV3;
Pj3=PP3-WW3*SS3*WW3';
MMXj(:,3)=X3;
%%%%%%%%%模型可能性計算%%%%%
A1=1/sqrt(det(2*pi*SS1))*exp(-1/2*VV1'*inv(SS1)*VV1);
A2=1/sqrt(det(2*pi*SS2))*exp(-1/2*VV2'*inv(SS2)*VV2);
A3=1/sqrt(det(2*pi*SS3))*exp(-1/2*VV3'*inv(SS3)*VV3);
%%%%%%%%%概率更新%%%%%%%%%
CC=A1*Cj(1)+A2*Cj(2)+A3*Cj(3);
u0(1)=A1*Cj(1)/CC;
u0(2)=A2*Cj(2)/CC;
u0(3)=A3*Cj(3)/CC;
%%%%%%%%模型輸出%%%%%%%%
MMX0(:,k)=MMXj(:,1)*u0(1)+MMXj(:,2)*u0(2)+MMXj(:,3)*u0(3);
end
運行出現
>> IMM_f
輸入引數的數目不足。
出錯 IMM_f (line 13)
NN=length(MX0(1,:));
uj5u.com熱心網友回復：
>> IMM_f輸入引數的數目不足。
出錯 IMM_f (line 13)
NN=length(MX0(1,:));
------------------
都提示引數不足了，去查一下length,MX0函式的用法。
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/116894.html
標籤：網絡通信