%參考文獻
%Decoupling?joint?probabilistic?data?association?algorithm?for?multiple?target?tracking?
%雜波環境下多傳感器的數據融合
%三維常速CV模型

clear?all;
clc;

T=1;?????????????????????????????????????????????????????%?采樣周期
hits=2000;???????????????????????????????????????????????%?采樣點數
MCNum=10;????????????????????????????????????????????????%?Monte?Carlo仿真次數
Qn=50;???????????????????????????????????????????????????%?觀測誤差標準差?
R_Q=50;??????????????????????????????????????????????????%?R方向觀測誤差標準差
THETA_Q=0.1;???????????????????????????????????????????????%?THETA方向觀測誤差標準差
PHI_Q=0.1;?????????????????????????????????????????????????%?PHI方向觀測誤差標準差
v_x=300;?v_y=200;?v_z=100;???????????????????????????????%?X、Y和Z方向的速度
x0=1000;?y0=5000;??z0=10000;???????????????????????????%?初始位置

DX_Average=zeros（1hits）;????????????????????????????????%?Monte?Carlo仿真的需要
DY_Average=zeros（1hits）;
DZ_Average=zeros（1hits）;

O2=[0?0;?0?0];???????????????????????????????????????????%?2*2階的零矩陣，為了以后賦值的方便
O3=[000]‘;?

f=[1?T;0?1];
F=[f?O2?O2;O2?f?O2;O2?O2?f];?????????????????????????????%?狀態轉移矩陣

q=[T^4/4?T^3/2;T^3/2?T^2];?
Q=[q?O2?O2;O2?q?O2;O2?O2?q];?????????????????????????????%?模型噪聲協方差陣

Qq=0.001;????????????????????????????????????????????????%?模擬加速度的過程噪聲方差
Q=Q*Qq;

%?H=[1?0?0?0?0?0?;0?0?1?0?0?0;0?0?0?0?1?0?];?????????????%?觀測矩陣

R=[R_Q^2?0?0;0?THETA_Q^2?0;0?0?PHI_Q^2];?????????????????????????%?觀測噪聲協方差陣

p1=[R_Q^2?0;0?1];
p2=[THETA_Q^2?0;0?1];
p3=[PHI_Q^2?0;0?1];

P0=[p1?O2?O2;O2?p1?O2;O2?O2?p1];???????????????????????????%?估計誤差協方差陣初值

X0=[x0v_xy0v_yz0v_z]‘;????????????????????%?狀態向量初值

%模擬軌跡
for?t=1:T:hits
????if（t==1）
????????X（t）=x0+v_x;
????????Y（t）=y0+v_y;
????????Z（t）=z0+v_z;
????else
????????X（t）=X（t-1）+v_x;
????????Y（t）=Y（t-1）+v_y;
????????Z（t）=Z（t-1）+v_z;
????end
end

[THETAPHIRo]?=?cart2sph（XYZ）;

%?Monte?Carlo?仿真的開始
for?i=1:1:MCNum

noise_x=Qn*randn（1hits）;
noise_y=Qn*randn（1hits）;
noise_z=Qn*randn（1hits）;


Z_X=X+noise_x;??????????????????????????????????????????%X方向加噪聲
Z_Y=Y+noise_y;??????????????????????????????????????????%Y方向加噪聲
Z_Z=Z+noise_z;??????????????????????????????????????????%Z方向加噪聲

noise_r=R_Q*randn（1hits）;
noise_theta=THETA_Q*randn（1hits）;
noise_phi=PHI_Q*randn（1hits）;


Z_R=Ro+noise_r;?????????????????????????????????????????????????????%X方向加噪聲
Z_THETA=THETA+noise_theta;??????????????????????????????????????????%Y方向加噪聲
Z_PHI=PHI+noise_phi;????????????????????????????????????????????????%Z方向加噪聲

Z_T=[Z_R;Z_THETA;Z_PHI];

%注意每次循環都要給X0和P0賦初值
X0=[x0v_xy0v_yz0v_z]‘;????????????????????????????????%?狀態向量初值
P0=[p1?O2?O2;O2?p1?O2;O2?O2?p1];???????????????????????????%估計誤差協方差陣初值

%EKF濾波方程
for?t=1:T:hits
????Xk_predict=F*X0;

????xm=Xk_predict（1）;
????ym=Xk_predict（3）;
????zm=Xk_predict（5）;

????r1=sqrt（xm^2+ym^2+zm^2）;
????r2=sqrt（xm^2+ym^2）;
????h1=[xm/r1?-ym/r2^2?-xm*zm/（r1^2*r2）]‘;
????h2=[ym/r1??xm/r2^2?-ym*zm/（r1^2*r2）]‘;
????h3=[zm/r1??0??

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

????I.A....?????43520??2006-01-10?19:57??ExtendedKalmanFilter\ekf.doc

????I.A....??????1690??2005-12-22?14:38??ExtendedKalmanFilter\ekf.rar

????I.A....??????4629??2005-09-30?21:28??ExtendedKalmanFilter\ekf_3dcv_filter.m

????I..D...?????????0??2006-01-10?19:57??ExtendedKalmanFilter

-----------?---------??----------?-----??----

????????????????50057????????????????????5