% UNTITLED3 the Summary of this function goes here
% the purpose of this function is to use the interactive multiple model algorithm for maneuvering target tracking
% % % parameters are as follows:
% % % MMX0, output target state
% % % sigmar, target position error standard deviation
Pt % % %, markov model transition probability, 3 * 3 matrix
% % % u0, prior probability model
% % % T, radar sampling interval
% % % MX0, target location measurement values
% %
NN=length (MX0 (1, :));
% % % % % % parameters initialization % % % % % % %
MMX0=zeros (6, NN);
% % % % using initialization first three measurement value % % %
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;
(MX0 MMX0 (3, 3)=(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 MX0-2 (2, 3) * (2, 2) + MX0 (2, 1))/T;
MMXj=[MMX0 (:, 3), MMX0 (:, 3), MMX0 (:, 3)]; % % % 3 model the initial state of
% % % % % % initialization of various new interest model covariance % % % % % % % %
R=sigmar ^ 2 * eye (2);
P11=R (1, 1);
P12=R/T (1, 1);
P13 (1, 1)=R/T ^ 2;
P22 (1, 1)=2 * R/T ^ 2;
P23=3 * R/T ^ 3 (1, 1);
P33 (1, 1)=6 * R/T ^ 4;
P44=R (2, 2);
P45=R/T (2, 2);
P46=R (2, 2)/T ^ 2;
P55 (2, 2)=2 * R/T ^ 2;
P56=3 * R/T ^ 3 (2, 2);
Have=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 have];
Pj2=Pj1;
Pj3=Pj1;
% % % % % initialization process noise covariance % % % % % %
: QQQ=[T ^ T ^ T ^ 4/8 5/20 3/6;
T ^ T ^ T ^ 4/8 3/3 2/2.
T ^ T ^ 3/6 2/2 T];
QQ=[: QQQ zeros (3), zeros (3) : QQQ];
Q1=10 * QQ; % % % model 1, q=10
Q2=QQ; % % % model 2, q=1
Q3=0.1 * QQ; % % % model 3, q=0.1
F1=[1 T 0.5 * T ^ 2; 1 T 0; 0, 0, 1].
F=[F1, zeros (3), zeros (3), F1]; % % % state matrix
H=[1 0 0 0 0 0; 1 0 0 0 0 0]; % % % measurement matrix
X1=MMXj (:, 1);
X2=MMXj (:, 2);
The X3=MMXj (:, 3); % % % iterative initial value
% %
% % % % % % % % % % % % % % iteration
For k=4: NN
% % % % % % % % % % % % % interaction
Cj=u0 * Pt;
For I=1:3
For j=1:3
Ukk (I, j)=Pt (I, j) * u0/Cj (I) (j);
End
End
MMXoj=MMXj * ukk; % % % % interaction state output
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; % % % % after interaction state variance output
% % % % % % % % model correction, kalman filtering % % % % % % % %
XX1 X1=F *;
ZZ1=H * XX1;
PP1 Poj1 * F=F * '+ Q1;
PP1 SS1=H * * * H '+ R;
VV1=MX0 (:, k) - ZZ1;
WW1 H '=PP1 * * inv (SS1).
X1=XX1 + WW1 * VV1;
Pj1=PP1 - SS1 WW1 * * WW1 ';
MMXj (:, 1)=X1;
XX2 X2=F *;
ZZ2=H * XX2;
PP2 Poj2 * F=F * '+ Q2;
SS2 PP2 * H=H * '+ R;
VV2=MX0 (:, k) - ZZ2;
WW2=PP2 * H '* inv (SS2);
X2=XX2 + WW2 * VV2;
SS2 Pj2=PP2 - WW2 * * WW2 ';
MMXj (:, 2)=X2,
XX3 X3=F *;
ZZ3=H * XX3;
PP3 Poj3 * F=F * '+ Q3;
SS3 PP3 * H=H * '+ R;
VV3=MX0 (:, k) - ZZ3;
WW3=PP3 * H '* inv (SS3);
The X3=XX3 + WW3 * VV3;
Pj3=PP3 - WW3 SS3 WW3 * * ';
MMXj (:, 3)=X3;
% % % % % % % % % probability calculation model % % % % %
A1=1/SQRT (det (2 * PI * SS1)) * * exp (- 1/2 VV1 '* inv (SS1) * VV1);
A2=1/SQRT (det (2 * PI * SS2)) * exp (VV2 '- 1/2 * * inv (SS2) * VV2);
A3=1/SQRT (det (2 * PI * SS3)) * * exp (- 1/2 VV3 '* inv (SS3) * VV3);
% % % % % % % % % probability updating % % % % % % % % %
CC=A1 * Cj (1) + A2 + A3 * * Cj (2) the Cj (3);
U0 (1)=A1 * Cj/CC (1);
U0 (2)=A2 * Cj/CC (2);
U0 (3)=A3 * Cj (3)/CC;
% % % % % % % % model output % % % % % % % %
MMX0 (:, k)=MMXj (:, 1) * u0 (1) + MMXj (:, 2) * u0 (2) + MMXj (:, 3) * u0 (3);
End
Run a
> IMM_f
The number of input parameters,
Error IMM_f (line 13)
NN=length (MX0 (1, :));
CodePudding user response:
> IMM_fThe number of input parameters,
Error IMM_f (line 13)
NN=length (MX0 (1, :));
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
Parameter is insufficient, suggest to check the length, MX0 function usage,