# 自己写的卡尔曼滤波程序

CKalmanfilter::CKalmanfilter(double delta, double T, double amax, double alpha)
{
//最大加速度
m_dAmax = amax;
//机动时间常数倒数，即机动频率
m_dAlpha = alpha;
//误差范围
m_dDelta = delta;
m_dDeltaMax = delta*20;
//时间步长，仿真时间间隔
m_dT = T;
//状态个数，//仿真次数
m_iIndex = 0;
//状态转移矩阵,赋初值的
m_mStateTrans = CMatrix(3,3);
m_mStateTrans.writedata(1.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(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); // 滤波的初始均方误差矩阵
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.writedata(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_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*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.writedata((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/2*(1-exp(-2*(pi/180)*(pi/180)));//////被动雷达角度测量噪声方差
double angle_pf=new double[N]; ///////被动雷

double angle_pf_yuce=new double[N]; ///////一步预测值
X=CMatrix(4,N);/////状态滤波值
XX=CMatrix(4,N);/////状态一步预测值
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 i=0;i<=3;i++)
{
W[i]=0;
for(int j=0;j<=N-1;j++)
{
X[i*N+j]=0;
XX[i*N+j]=0;
G[i*N+j]=0;
H[i+j*4]=0;
}
}

A[0]=1;
A[1]=0;
A[2]=1;
A[3]=0;
A[4]=0;
A[5]=1;
A[6]=0;
A[7]=1;
A[8]=0;
A[9]=0;
A[10]=1;
A[11]=0;
A[12]=0;
A[13]=0;
A[14]=0;
A[15]=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 (i==j)
E[i*4+j]=1;
else
E[i*4+j]=0;
}
} */

for(int k=0;k<=delay-1;k++)
{
H[k*4]=cos(angle_p_mea[k]);
H[k*4+1]=-sin(angle_p_mea[k]);
H[k*4+2]=0;
H[k*4+3]=0;

if (k==0)
{
for(int i=0;i<=3;i++)
{
X[i*4+k]=0;
for (int i=0;i<=3;i++)
{
if (int j=4*i+1)
p[j]=1;
else
p[j]=0;
}
for(int j=0;j<=3;j++)
XX[i*4+k]+=A[i*4+j]*X[j*4+i];
}
angle_pf_yuce(k)=atan2(XX[K],XX[4+K]);
}
else
{
W(0)=1/2*T*T*v_missile*(sin(angle_p_real(k))-sin(angle_p_real(k-1)));///导弹航向变化，产生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<=3;i++)
{
/// ????pp
/// ????G
X[i*N+k]=XX[i*N+k-1]-G[i*N+k]*H[k*4+i]*XX[i*N+k-1];
/// ????p
for(int j=0;j<=3;j++)
{
XX[i*4+k]+=A[i*4+j]*X[j*4+i];
XX[i*4+k]=XX[i*4+k]+W[i];
}
angle_pf_yuce[k]=atan2(XX[k],XX[4+k]);
}
}
}
////////将滤波后的rx,ry转化为观测角
for(int i=0;i<=delay-1;i++)
{
if (X[4+i]!=0)
angle_pf[i]=atan2[X[i],X[4+i]];
else
angle_pf[i]=angle_p_mea[i];

xita1[0]=pi/2-angle_p_mea[0];
/////滤波轨迹（与真实轨迹对比）
x_t[0]=x0;
y_t[0]=y0;
for(int i=1;i<=N-1;i++)
{
x_t[i]=x_t[i-1]+T*v_target*cos(angle_t_x);
y_t[i]=y_t[i-1]+T*v_target*sin(angle_t_x);
}
if (i==0)
{
double deta_angle=angle_pf[i];
x_m[0]=x_m0;
y_m[0]=y_m0;
x_m[1]=x_m[0]+T*v_missile*sin(deta_angle);
y_m[1]=y_m[0]+T*v_missile*cos(deta_angle);
}
else
{
if (i<600)
xita1[i]=xita1[i-1]+N_zhidao*((pi/2-angle_p_mea[i])-(pi/2-angle_p_mea[i-1]))/T;
else
xita1[i]=xita1[i-1]+N_zhidao*((pi/2-angle_pf[i])-(p

i/2-angle_pf[i-1]))/T;
x_m[i]=x_m[i-1]+T*v_target*cos(xita1[i]);
y_m[i]=y_m[i-1]+T*v_target*sin(xita1[i]);
}
}