imm_init_model.m

IMM PDAF跟踪滤波的程序

function [Fa,Ha,Qa,Ra,InitXa,InitVa,P] = IMM_Init_Model(dT,M,ModelDim)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% IMM_Init_Model initializes the IMM.
% It uses M number of models
% Input:
%   dT  - sampling interval
%   M   - number of models to intialize (default 3)
% Output:
%   Fa   - ss x ss x M array of transition matrix
%   Ha   - os x ss x M array of observation matrix
%   Qa  - ss x ss x M array of state covariances
%   Ra  - os x ss x M array of measurement covariances
%   InitXa  - initial position array
%   InitVa  - initial variance array
%   P       - M x M interaction prob. matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 3, ModelDim = 2; end;
if nargin <2, M = 3; end;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Init parameters
ProbDim  = 2;
[F,H,Q,R,ObservInd] = Kalman_Filter_Init(dT,ModelDim,ProbDim);
initx   = zeros(size(F,1),1);
initV   = Q;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% noise parameters
SigmaQ  = logspace(.01,1,M)*1;
SigmaR  = logspace(.01,1,M)*.01;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% model interaction
switch M,
case 1,
   % P = [0.8 0.1 0.15;0.1 0.6 0.05; 0.1 0.3 0.8];
    P = 1;
case 2,
   % P = [0.8 0.1 0.15;0.1 0.6 0.05; 0.1 0.3 0.8];
    P = [.9 .1;.1 .9];
%    P = [.9 .3;.1 .7];
case 3,
    P = [.7 .2 .1; .2 .6 .2; .1 .2 .7];
%    P = [0.8 0.1 0.15;0.1 0.6 0.05; 0.1 0.3 0.8];
    %P = ones(3,3)/3;
otherwise
    P = ones(M,M)./M;   
    warning('Interaction matrix for this number of models does not exists! Creating  uniform...')
end;
    
%if ~all(sum(P)==1), error('P is not valid transition matrix'); end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% init.
for i=1:M,
   Fa(:,:,i)     = F;
   Ha(:,:,i)     = H;
   Qa(:,:,i)     = SigmaQ(i).*Q;
   Ra(:,:,i)     = SigmaR(i).*R;
   InitXa(:,i)   = initx;
   InitVa(:,:,i) = initV;
end;