www.pudn.com > Kalman--C++.rar > Singer.cpp, change:2004-02-19,size:11529b
// Singer.cpp: implementation of the CSinger class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "Singer.h"
#include "stdlib.h"
#include "math.h"
#include "iostream.h"
#include "Matrix.h"
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CSinger::CSinger()
{
srand((unsigned)time( NULL ) );
int i;
cov=100;
T=2;
alfa=1.0/60;
cova=0.03;
for(i=0;i<350;i++)
{
A_Real[i][0]=0;
A_Real[i][1]=0;
V_Real[i][0]=0;
V_Real[i][1]=0;
XY_Real[i][0]=0;
XY_Real[i][1]=0;
XY_Filt[i][0]=0;
XY_Filt[i][1]=0;
}
GenerateRealTrack();
}
CSinger::~CSinger()
{
}
void CSinger::AddNoise()//产生正态白噪声
{
double r1,r2;
double noise;
int i;
for(i=0;i<350;i++)
{
r1=(double)rand()/RAND_MAX;
r2=(double)rand()/RAND_MAX;
noise=cov*sqrt(-2*log(r1))*cos(2*PIE*r2);
XY_Obsv[i][0]=XY_Real[i][0]+noise;
}
// srand((unsigned)time( NULL ));
for(i=0;i<350;i++)
{
r1=(double)rand()/RAND_MAX;
r2=(double)rand()/RAND_MAX;
noise=cov*sqrt(-2*log(r1))*cos(2*PIE*r2);
XY_Obsv[i][1]=XY_Real[i][1]+noise;
}
}
void CSinger::GenerateRealTrack()//产生真实的轨迹
{
int i;
//加速度的初始值
for(i=0;i<200;i++)
{
A_Real[i][0]=0;
A_Real[i][1]=0;
}
for(i=200;i<300;i++)
{
A_Real[i][0]=0.075;
A_Real[i][1]=0.075;
}
for(i=300;i<305;i++)
{
A_Real[i][0]=0;
A_Real[i][1]=0;
}
for(i=305;i<330;i++)
{
A_Real[i][0]=-0.3;
A_Real[i][1]=-0.3;
}
for(i=330;i<350;i++)
{
A_Real[i][0]=0;
A_Real[i][1]=0;
}
//速度的初始值
for(i=0;i<200;i++)
{
V_Real[i][0]=0;
V_Real[i][1]=-15;
}
//位置的初始值
XY_Real[0][0]=2000;
XY_Real[0][1]=10000;
//计算速度
for(i=0;i<349;i++)
{
V_Real[i+1][0]=V_Real[i][0]+T*A_Real[i][0];
V_Real[i+1][1]=V_Real[i][1]+T*A_Real[i][1];
}
//计算位置坐标
for(i=0;i<349;i++)
{
XY_Real[i+1][0]=XY_Real[i][0]+T*V_Real[i][0]+0.5*pow(T,2)*A_Real[i][0];
XY_Real[i+1][1]=XY_Real[i][1]+T*V_Real[i][1]+0.5*pow(T,2)*A_Real[i][1];
}
}
void CSinger::Filter()//kalman滤波
{
AddNoise();
int k=3;
CMatrix I(6,6);//单位矩阵
I(1,1)=1;
I(2,2)=1;
I(3,3)=1;
I(4,4)=1;
I(5,5)=1;
I(6,6)=1;
CMatrix X(6,1);//状态矩阵
X(1,1)=XY_Obsv[1][0];
X(2,1)=(XY_Obsv[1][0]-XY_Obsv[0][0])/T;
X(4,1)=XY_Obsv[1][1];
X(5,1)=(XY_Obsv[1][1]-XY_Obsv[0][1])/T;
// cout<<"X(2)"<观测
H(1,1)=1;
H(2,4)=1;
// cout<<"H"<MAX_SPEED||(XY_Obsv[k][1]-XY_Obsv[k-1][1])/T>MAX_SPEED)
{
Z(1,1)=XY_Obsv[k-1][0];
Z(2,1)=XY_Obsv[k-1][1];
// XY_Obsv[k][0]=XY_Obsv[k-1][0];
// XY_Obsv[k][1]=XY_Obsv[k-1][1];
}
else
{
Z(1,1)=XY_Obsv[k][0];
Z(2,1)=XY_Obsv[k][1];
}
X=Fai*X;
// X.Display();
P=Fai*P*~Fai+Q;
// P.Display();
K=P*~H*!(H*P*~H+R);
// K.Display();
X=X+K*(Z-H*X);
// cout<观测
H(1,1)=1;
H(2,4)=1;
// cout<<"H"<MAX_SPEED||(XY_Obsv[k][1]-XY_Obsv[k-1][1])/T>MAX_SPEED)
{
Z(1,1)=XY_Obsv[k-1][0];
Z(2,1)=XY_Obsv[k-1][1];
// XY_Obsv[k][0]=XY_Obsv[k-1][0];
// XY_Obsv[k][1]=XY_Obsv[k-1][1];
}
else
{
Z(1,1)=XY_Obsv[k][0];
Z(2,1)=XY_Obsv[k][1];
}
X=Fai1*X;
// X.Display();
P=Fai*P*~Fai+Q;
// P.Display();
K=P*~H*!(H*P*~H+R);
// K.Display();
X=X+K*(Z-H*X);
// cout<观测
CMatrix H2(2,4);//状态->观测初始值
H2(1,1)=1;
H2(1,2)=T;
H2(2,3)=1;
H2(2,4)=T;
// H(1,1)=1;
// H(1,2)=T;
// H(2,3)=1;
// H(2,4)=T;
CMatrix Z(2,1);//观测值
CMatrix R(2,2);//观测噪声协方差阵
R(1,1)=cov*cov;
R(2,2)=cov*cov;
CMatrix K(4,2);//增益
CMatrix P(4,4);//预测误差,滤波协方差阵
P=!(~H2*!R*H2);
CMatrix X(2,1);
//头两个滤波值就是观测值
XY_Filt[0][0]=XY_Obsv[0][0];
XY_Filt[0][1]=XY_Obsv[0][1];
XY_Filt[1][0]=XY_Obsv[1][0];
XY_Filt[1][1]=XY_Obsv[1][1];
for(k=2;k<350;k++)
{
if((XY_Obsv[k][0]-XY_Obsv[k-1][0])/T>MAX_SPEED||(XY_Obsv[k][1]-XY_Obsv[k-1][1])/T>MAX_SPEED)
{
Z(1,1)=XY_Obsv[k-1][0];
Z(2,1)=XY_Obsv[k-1][1];
// XY_Obsv[k][0]=XY_Obsv[k-1][0];
// XY_Obsv[k][1]=XY_Obsv[k-1][1];
}
else
{
Z(1,1)=XY_Obsv[k][0];
Z(2,1)=XY_Obsv[k][1];
}
H(1,1)=1;
H(1,2)=k*T;
H(2,3)=1;
H(2,4)=k*T;
K=P*~H*!(H*P*~H+R);
X0=X0+K*(Z-H*X0);
P=(I-K*H)*P;
X=H*X0;
XY_Filt[k][0]=X(1,1);
XY_Filt[k][1]=X(2,1);
}
}
void CSinger::CalErrorLms(int M)//计算lms滤波器误差的均值、标准差
{
int i,j;
for(i=0;i<350;i++)
{
ex[i]=0;
ey[i]=0;
dx[i]=0;
dy[i]=0;
}
GenerateRealTrack();
for(i=0;i