資源簡介
IMM 濾波算法 多模型 代碼

代碼片段和文件信息
clear?all;
clc;
echo?off;
%===============================
%建立模型
%===============================
%?仿真參數(shù)
simTime=100;??????%仿真迭代次數(shù)
T=1;?????????????????????%采樣時間
w2=3*2*pi/360;?????%模型2轉彎率3度
w3=-3*2*pi/360;????%模型3轉彎率-3度
H=[1000;0010];??????????????????????%模型量測矩陣
G=[T^2/20;T0;0T^2/2;0T];??????????????%模型過程噪聲加權矩陣
r=200;?????????????????????????????????%20?2000
R=[r0;0r];????????????????????????????%模型量測噪聲協(xié)方差矩陣
Q=[100;010];??????????????????????????????????%模型過程噪聲協(xié)方差矩陣
F1=[1T00;0100;001T;0001];?????%模型1狀態(tài)轉移矩陣
F2=[1sin(w2*T)/w20(cos(w2*T)-1)/w2;
????0cos(w2*T)0sin(w2*T);
????0(1-cos(w2*T))/w21sin(w2*T)/w2;
????0-sin(w2*T)0cos(w2*T)];????????????%模型2狀態(tài)轉移矩陣?左轉彎
F3=[1sin(w3*T)/w30(cos(w3*T)-1)/w3;
????0cos(w3*T)0sin(w3*T);
????0(1-cos(w3*T))/w31sin(w3*T)/w3;
????0-sin(w3*T)0cos(w3*T)];????????????%模型3狀態(tài)轉移矩陣?右轉彎
w4=3*2*pi/360;?????%模型2轉彎率3度
w5=-3*2*pi/360;????%模型3轉彎率-3度
F4=[1sin(w4*T)/w40(cos(w4*T)-1)/w4;
????0cos(w4*T)0sin(w4*T);
????0(1-cos(w4*T))/w41sin(w4*T)/w4;
????0-sin(w4*T)0cos(w4*T)];????????????%模型2狀態(tài)轉移矩陣
F5=[1sin(w5*T)/w50(cos(w5*T)-1)/w5;
????0cos(w5*T)0sin(w5*T);
????0(1-cos(w5*T))/w51sin(w5*T)/w5;
????0-sin(w5*T)0cos(w5*T)];????????????%模型3狀態(tài)轉移矩陣
x0=[10002001000200]‘;??%?初始狀態(tài)
%?產(chǎn)生量測數(shù)據(jù)
%[zz_true]=targets();
randn(‘state‘sum(100*clock));?%?Shuffle?the?pack!
x?=?zeros(4simTime);
z?=?zeros(2simTime);?????????%含噪聲量測數(shù)據(jù)
z_true?=?zeros(2simTime);????%真值數(shù)據(jù)
measureNoise?=?zeros(2simTime);
measureNoise?=?sqrt(R)*randn(2simTime);??%產(chǎn)生量測噪聲
x(:1)=x0;
z(:1)=H*x(:1)+measureNoise(:1);
z_true(:1)=H*x(:1);
for?a=2:simTime
????if?(a>=20)&&(a<=40)?
????????x(:a)=F4*x(:a-1);??????%20--40s左轉?
????elseif?(a>=60)&&(a<=80)?
???????x(:a)=F5*x(:a-1);????????%60--80s右轉?
????else
????????x(:a)=F1*x(:a-1);??????%勻速直線運動
????end;?
z(:a)=H*x(:a)+measureNoise(:a);
z_true(:a)=H*x(:a);
end;
%===============================
%?????IMM
%===============================
%初始化
x1_IMM?=?zeros(41);??????%模型1IMM算法狀態(tài)估計值
x2_IMM?=?zeros(41);??????%模型2IMM算法狀態(tài)估計值
x3_IMM?=?zeros(41);??????%模型3IMM算法狀態(tài)估計值
x_pro_IMM?=?zeros(4simTime);???%IMM算法模型綜合狀態(tài)估計值
P_IMM=zeros(44simTime);???????%IMM算法模型綜合狀態(tài)協(xié)方差矩陣
P1_IMM=zeros(44);
P2_IMM=zeros(44);
P3_IMM=zeros(44);??????????????%IMM算法各模型協(xié)方差矩陣
r1_IMM=zeros(21);
r2_IMM=zeros(21);
r3_IMM=zeros(21);
S1_IMM=zeros(22);
S2_IMM=zeros(22);
S3_IMM=zeros(22);
%初始化
x_pro_IMM(:1)=x0;
pij=[0.90.050.05;
????0.10.80.1;
????0.050.150.8];????%模型轉移概率矩陣
%pij=[0.60.20.2;0.20.60.2;0.250.150.6];????%模型轉移概率矩陣
u_IMM=zeros(3simTime);
u_IMM(:1)=[0.30.30.4]‘;??%IMM算法模型概率
x1_IMM=x0;x2_IMM=x0;x3_IMM=x0;??%IMM算法各模型初始狀態(tài)
P0=diag([10005001000500]);??%初始狀態(tài)協(xié)方差矩陣
P1_IMM=P0;P2_IMM=P0;P3_IMM=P0;
P_IMM(::1)=P0;
%main?loop
for?t=1:simTime-1
????
????%第一步Interacting(只針對IMM算法)
????c_j=pij‘*u_IMM(:t);
????
????ui1=(1/c_j(1))*pij(:1).*u_IMM(:t);
????ui2=(1/c_j(2))*pij(:2).*u_IMM(:t);
????ui3=(1/c_j(3))*pij(:3).*u_IMM(:t);????%計算模型混合概率
????
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????5751??2012-04-30?15:03??IMM代碼\IMMKF_Matlab.m
?????文件?????????985??2012-04-30?14:49??IMM代碼\Kalman.m
?????文件?????????796??2012-04-30?14:48??IMM代碼\Model_P_up.m
?????文件?????????472??2012-04-30?14:49??IMM代碼\Model_mix.m
?????目錄???????????0??2018-08-29?16:01??IMM代碼\
- 上一篇:帶校驗的CRC串口文件傳輸
- 下一篇:ETC打包工具解包工具
評論
共有 條評論