www.pudn.com > un_IMM_PDAF.zip > IMM_Init_Model.m

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;