自己寫的卡爾曼濾波程序

1、CKalmanfilter:CKalmanfilter(double delta, double T, double amax, double alpha /最大加速度 m_dAmax = amax; /機動時間常數(shù)倒數(shù)，即機動頻率 m_dAlpha = alpha; /誤差范圍 m_dDelta = delta; m_dDeltaMax = delta*20; /時間步長，仿真時間間隔 m_dT = T; /狀態(tài)個數(shù)，/仿真次數(shù) m_iIndex = 0; /狀態(tài)轉移矩陣,賦初值的 m_mStateTrans = CMatrix(3,3; m_mStateTrans.writedata(1.

2、0,0,0; m_mStateTrans.writedata(T,0,1; m_mStateTrans.writedata(m_dAlpha*m_dT-1.0+exp(-m_dAlpha*m_dT/(m_dAlpha*m_dAlpha,0,2; m_mStateTrans.writedata(0,1,0; m_mStateTrans.writedata(1.0,1,1; m_mStateTrans.writedata(1-exp(-m_dAlpha*m_dT/m_dAlpha,1,2; m_mStateTrans.writedata(0,2,0; m_mStateTrans.writedata

3、(0,2,1; m_mStateTrans.writedata(exp(-m_dAlpha*m_dT,2,2; / m_mStateTrans.out(; m_mMeasure = CMatrix(1,3; /量測矩陣 m_mMeasure.writedata(1.0,0,0; / m_mMeasure.out(; m_mI = CMatrix(3,3; /單位矩陣 m_mI.writedata(1,0,0; m_mI.writedata(1,1,1; m_mI.writedata(1,2,2; / m_mI.out(; m_mMSEfilter = CMatrix(3,3; / 濾波的初始均

4、方誤差矩陣 m_mMSEfilter.writedata(m_dDelta*m_dDelta,0,0; m_mMSEfilter.writedata(m_mMSEfilter.getdata(0,0/m_dT,0,1; m_mMSEfilter.writedata(m_mMSEfilter.getdata(0,0/m_dT/m_dT,0,2; m_mMSEfilter.writedata(m_mMSEfilter.getdata(0,1,1,0; m_mMSEfilter.writedata(2*m_dDelta*m_dDelta/m_dT/m_dT,1,1; m_mMSEfilter.wri

5、tedata(m_mMSEfilter.getdata(1,1/m_dT,1,2; m_mMSEfilter.writedata(m_mMSEfilter.getdata(0,2,2,0; m_mMSEfilter.writedata(m_mMSEfilter.getdata(1,2,2,1; m_mMSEfilter.writedata(m_mMSEfilter.getdata(0,1,1,0; m_mMSEfilter.writedata(6*m_dDelta*m_dDelta/m_dT/m_dT/m_dT/m_dT,2,2; /誤差矩陣 m_mError = CMatrix(3,3; m

6、_mError.writedata(1-exp(-2*m_dAlpha*m_dT+2*m_dAlpha*m_dT+2.0/3.0*pow(m_dAlpha*m_dT,3.0-2.0*pow(m_dAlpha*m_dT,2.0-4*m_dAlpha*m_dT*exp(-1*m_dAlpha*m_dT/(2.0*pow(m_dAlpha,5.0,0,0; m_mError.writedata(1+exp(-2*m_dAlpha*m_dT-2*m_dAlpha*m_dT-2*exp(-1*m_dAlpha*m_dT+pow(m_dAlpha*m_dT,2+2*m_dAlpha*m_dT*exp(-1

7、*m_dAlpha*m_dT/(2*pow(m_dAlpha,4,0,1; m_mError.writedata(1-exp(-2*m_dAlpha*m_dT-2*m_dAlpha*m_dT*exp(-1*m_dAlpha*m_dT/(2*pow(m_dAlpha,3,0,2; m_mError.writedata(m_mError.getdata(0,1,1,0; m_mError.writedata(4*exp(-1*m_dAlpha*m_dT-3-exp(-2*m_dAlpha*m_dT+2*m_dAlpha*m_dT/(2*pow(m_dAlpha,3,1,1; m_mError.wr

8、itedata(1+exp(-2*m_dAlpha*m_dT-2*exp(-1*m_dAlpha*m_dT/(2*m_dAlpha*m_dAlpha,1,2; m_mError.writedata(m_mError.getdata(0,2,2,0; m_mError.writedata(m_mError.getdata(1,2,2,1; m_mError.writedata(1-exp(-2*m_dAlpha*m_dT/(2*m_dAlpha,2,2; / m_mError.out(; /matlab 方法的卡爾曼濾波 double pi=3.1415926; double deta_p=1/

9、2*(1-exp(-2*(pi/180*(pi/180;/被動雷達角度測量噪聲方差 double angle_pf=new doubleN; /被動雷 達角度濾波值 double angle_pf_yuce=new doubleN; /一步預測值 X=CMatrix(4,N;/狀態(tài)濾波值 XX=CMatrix(4,N;/狀態(tài)一步預測值 P=CMatrix(;/濾波均方誤差陣 PP=CMatrix(;/一步預測均方誤差陣 H=CMatrix(N,4; G=CMatrix(4,N; A=CMatrix(4,1; W=CMatrix(4,4; I=CMatrix(4,4;/單位矩陣 for(int

10、i=0;i<=3;i+ Wi=0; for(int j=0;j<=N-1;j+ Xi*N+j=0; XXi*N+j=0; Gi*N+j=0; Hi+j*4=0; A0=1; A1=0; A2=1; A3=0; A4=0; A5=1; A6=0; A7=1; A8=0; A9=0; A10=1; A11=0; A12=0; A13=0; A14=0; A15=1; CMatrix TrsOfA=new double 4,4;/矩陣A的轉置矩陣 TrsOfA=transpose(A; /* for(int i=0;i<=3;i+ for(int j=0;j<=3;j+ if

11、 (i=j Ei*4+j=1; else Ei*4+j=0; */ for(int k=0;k<=delay-1;k+ Hk*4=cos(angle_p_meak; Hk*4+1=-sin(angle_p_meak; Hk*4+2=0; Hk*4+3=0; if (k=0 for(int i=0;i<=3;i+ Xi*4+k=0; for (int i=0;i<=3;i+ if (int j=4*i+1 pj=1; else pj=0; for(int j=0;j<=3;j+ XXi*4+k+=Ai*4+j*Xj*4+i; angle_pf_yuce(k=atan2(X

12、XK,XX4+K; else W(0=1/2*T*T*v_missile*(sin(angle_p_real(k-sin(angle_p_real(k-1;/導彈航向變化，產(chǎn)生x、y方向加速度； W(1=1/2*T*T*v_missile*(cos(angle_p_real(k-cos(angle_p_real(k-1; W(2=T*T*v_missile*(sin(angle_p_real(k-sin(angle_p_real(k-1; W(3=T*T*v_missile*(sin(angle_p_real(k-sin(angle_p_real(k-1; for(int i=0;i<=

13、3;i+ / ?pp / ?G Xi*N+k=XXi*N+k-1-Gi*N+k*Hk*4+i*XXi*N+k-1; / ?p for(int j=0;j<=3;j+ XXi*4+k+=Ai*4+j*Xj*4+i; XXi*4+k=XXi*4+k+Wi; angle_pf_yucek=atan2(XXk,XX4+k; /將濾波后的rx,ry轉化為觀測角 for(int i=0;i<=delay-1;i+ if (X4+i!=0 angle_pfi=atan2Xi,X4+i; else angle_pfi=angle_p_meai; xita10=pi/2-angle_p_mea0; /濾波軌跡（與真實軌跡對比） x_t0=x0; y_t0=y0; for(int i=1;i<=N-1;i+ x_ti=x_ti-1+T*v_target*cos(angle_t_x; y_ti=y_ti-1+T*v_target*sin(angle_t_x; if (i=0 double deta_angle=angle_pfi; x_m0=x_m0; y_m0=y_m0; x_m1=x_m0+T*v_missile*sin(deta_angle; y_m1=y_m0+T*v_missile*cos(deta_angle; else