?? singer.cpp
字號:
// 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)"<<endl;
// X.Display();
/* CMatrix Fai1(6,6);//狀態轉移矩陣
Fai1(1,1)=1;
Fai1(2,2)=1;
Fai1(3,3)=1;
Fai1(1,2)=T;
Fai1(1,3)=0.5*pow(T,2);
Fai1(2,3)=T;
Fai1(4,4)=1;
Fai1(5,5)=1;
Fai1(6,6)=1;
Fai1(4,5)=T;
Fai1(4,6)=0.5*pow(T,2);
Fai1(5,6)=T;
cout<<"Fai1"<<endl;
Fai1.Display();
*/
CMatrix Fai(6,6);//狀態轉移矩陣
Fai(1,1)=1;
Fai(2,2)=1;
Fai(3,3)=exp(-alfa*T);
Fai(1,2)=T;
Fai(1,3)=1/pow(alfa,2)*(-1+alfa*T+exp(-alfa*T));
Fai(2,3)=1/alfa*(1-exp(-alfa*T));
Fai(4,4)=1;
Fai(5,5)=1;
Fai(6,6)=exp(-alfa*T);
Fai(4,5)=T;
Fai(4,6)=1/pow(alfa,2)*(-1+alfa*T+exp(-alfa*T));
Fai(5,6)=1/alfa*(1-exp(-alfa*T));
// cout<<"Fai"<<endl;
// Fai.Display();
CMatrix H(2,6);//狀態->觀測
H(1,1)=1;
H(2,4)=1;
// cout<<"H"<<endl;
// H.Display();
CMatrix Z(2,1);//觀測值
CMatrix R(2,2);//觀測噪聲協方差陣
R(1,1)=pow(cov,2);
R(2,2)=pow(cov,2);
CMatrix Q(6,6);//擾動噪聲協方差陣
Q(1,1)=2*alfa*cova*cova*pow(T,5)/20;
Q(1,2)=2*alfa*cova*cova*pow(T,4)/8;
Q(1,3)=2*alfa*cova*cova*pow(T,3)/6;
Q(2,1)=2*alfa*cova*cova*pow(T,4)/8;
Q(2,2)=2*alfa*cova*cova*pow(T,3)/3;
Q(2,3)=2*alfa*cova*cova*pow(T,2)/2;
Q(3,1)=2*alfa*cova*cova*pow(T,3)/6;
Q(3,2)=2*alfa*cova*cova*pow(T,2)/2;
Q(3,3)=2*alfa*cova*cova*T;
Q(4,4)=2*alfa*cova*cova*pow(T,5)/20;
Q(4,5)=2*alfa*cova*cova*pow(T,4)/8;
Q(4,6)=2*alfa*cova*cova*pow(T,3)/6;
Q(5,4)=2*alfa*cova*cova*pow(T,4)/8;
Q(5,5)=2*alfa*cova*cova*pow(T,3)/3;
Q(5,6)=2*alfa*cova*cova*pow(T,2)/2;
Q(6,4)=2*alfa*cova*cova*pow(T,3)/6;
Q(6,5)=2*alfa*cova*cova*pow(T,2)/2;
Q(6,6)=2*alfa*cova*cova*T;
// cout<<"Q"<<endl;
// Q.Display();
CMatrix K(6,2);//kalman增益
CMatrix P(6,6);//預測誤差,濾波協方差陣
P(1,1)=pow(cov,2);
P(1,2)=pow(cov,2)/T;
P(2,1)=P(1,2);
P(2,2)=2*pow(cov,2)/pow(T,2);
P(4,4)=pow(cov,2);
P(4,5)=pow(cov,2)/T;
P(5,4)=P(4,5);
P(5,5)=2*pow(cov,2)/pow(T,2);
// cout<<"P(2)"<<endl;
// P.Display();
//頭兩個濾波值就是觀測值
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];
}
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<<endl;
// cout<<XY_Real[k][0]<<" "<<XY_Real[k][1]<<endl;
// cout<<XY_Obsv[k][0]<<" "<<XY_Obsv[k][1]<<endl;
// cout<<X(1,1)<<" "<<X(4,1)<<endl;;
P=(I-K*H)*P;
// P.Display();
XY_Filt[k][0]=X(1,1);
XY_Filt[k][1]=X(4,1);
}
}
void CSinger::Filter_2()//kalman_Singer算法濾波(第二種方法)
{
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)"<<endl;
// X.Display();
CMatrix Fai1(6,6);//狀態轉移矩陣
Fai1(1,1)=1;
Fai1(2,2)=1;
Fai1(3,3)=1;
Fai1(1,2)=T;
Fai1(1,3)=0.5*pow(T,2);
Fai1(2,3)=T;
Fai1(4,4)=1;
Fai1(5,5)=1;
Fai1(6,6)=1;
Fai1(4,5)=T;
Fai1(4,6)=0.5*pow(T,2);
Fai1(5,6)=T;
// cout<<"Fai1"<<endl;
// Fai1.Display();
CMatrix Fai(6,6);//狀態轉移矩陣
Fai(1,1)=1;
Fai(2,2)=1;
Fai(3,3)=exp(-alfa*T);
Fai(1,2)=T;
Fai(1,3)=1/pow(alfa,2)*(-1+alfa*T+exp(-alfa*T));
Fai(2,3)=1/alfa*(1-exp(-alfa*T));
Fai(4,4)=1;
Fai(5,5)=1;
Fai(6,6)=exp(-alfa*T);
Fai(4,5)=T;
Fai(4,6)=1/pow(alfa,2)*(-1+alfa*T+exp(-alfa*T));
Fai(5,6)=1/alfa*(1-exp(-alfa*T));
// cout<<"Fai"<<endl;
// Fai.Display();
CMatrix H(2,6);//狀態->觀測
H(1,1)=1;
H(2,4)=1;
// cout<<"H"<<endl;
// H.Display();
CMatrix Z(2,1);//觀測值
CMatrix R(2,2);//觀測噪聲協方差陣
R(1,1)=pow(cov,2);
R(2,2)=pow(cov,2);
CMatrix Q(6,6);//擾動噪聲協方差陣
Q(1,1)=2*alfa*pow(cova,2)*pow(T,5)/20;
Q(1,2)=2*alfa*pow(cova,2)*pow(T,4)/8;
Q(1,3)=2*alfa*pow(cova,2)*pow(T,3)/6;
Q(2,1)=2*alfa*pow(cova,2)*pow(T,4)/8;
Q(2,2)=2*alfa*pow(cova,2)*pow(T,3)/3;
Q(2,3)=2*alfa*pow(cova,2)*pow(T,2)/2;
Q(3,1)=2*alfa*pow(cova,2)*pow(T,3)/6;
Q(3,2)=2*alfa*pow(cova,2)*pow(T,2)/2;
Q(3,3)=2*alfa*pow(cova,2)*T;
Q(4,4)=2*alfa*pow(cova,2)*pow(T,5)/20;
Q(4,5)=2*alfa*pow(cova,2)*pow(T,4)/8;
Q(4,6)=2*alfa*pow(cova,2)*pow(T,3)/6;
Q(5,4)=2*alfa*pow(cova,2)*pow(T,4)/8;
Q(5,5)=2*alfa*pow(cova,2)*pow(T,3)/3;
Q(5,6)=2*alfa*pow(cova,2)*pow(T,2)/2;
Q(6,4)=2*alfa*pow(cova,2)*pow(T,3)/6;
Q(6,5)=2*alfa*pow(cova,2)*pow(T,2)/2;
Q(6,6)=2*alfa*pow(cova,2)*T;
// cout<<"Q"<<endl;
// Q.Display();
CMatrix K(6,2);//kalman增益
CMatrix P(6,6);//預測誤差,濾波協方差陣
P(1,1)=pow(cov,2);
P(1,2)=pow(cov,2)/T;
P(2,1)=P(1,2);
P(2,2)=2*pow(cov,2)/pow(T,2);
P(4,4)=pow(cov,2);
P(4,5)=pow(cov,2)/T;
P(5,4)=P(4,5);
P(5,5)=2*pow(cov,2)/pow(T,2);
// cout<<"P(2)"<<endl;
// P.Display();
//頭兩個濾波值就是觀測值
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];
}
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<<endl;
// cout<<XY_Real[k][0]<<" "<<XY_Real[k][1]<<endl;
// cout<<XY_Obsv[k][0]<<" "<<XY_Obsv[k][1]<<endl;
// cout<<X(1,1)<<" "<<X(4,1)<<endl;;
P=(I-K*H)*P;
// P.Display();
XY_Filt[k][0]=X(1,1);
XY_Filt[k][1]=X(4,1);
}
}
void CSinger::CalError(int M,BOOL kind)//計算濾波器誤差的均值、標準差
{
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<M;i++)
{
if(kind==0)
Filter();
else
Filter_2();
for(j=0;j<350;j++)
{
ex[j]+=XY_Real[j][0]-XY_Filt[j][0];
ey[j]+=XY_Real[j][1]-XY_Filt[j][1];
dx[j]+=pow(XY_Real[j][0]-XY_Filt[j][0],2);
dy[j]+=pow(XY_Real[j][1]-XY_Filt[j][1],2);
}
}
for(j=0;j<350;j++)
{
ex[j]=ex[j]/M;
ey[j]=ey[j]/M;
dx[j]=sqrt(dx[j]/M-ex[j]);
dy[j]=sqrt(dy[j]/M-ey[j]);
}
for(j=0;j<350;j++)
{
cout<<ex[j]<<" "<<ey[j]<<endl;
cout<<dx[j]<<" "<<dy[j]<<endl;
}
}
void CSinger::Filter_LMS()//最小二乘遞推估計
{
AddNoise();
int k;
CMatrix I(4,4);//單位矩陣
I(1,1)=1;
I(2,2)=1;
I(3,3)=1;
I(4,4)=1;
CMatrix X0(4,1);//狀態矩陣
X0(1,1)=XY_Obsv[0][0];
X0(2,1)=(XY_Obsv[1][0]-XY_Obsv[0][0])/T;
X0(3,1)=XY_Obsv[0][1];
X0(4,1)=(XY_Obsv[1][1]-XY_Obsv[0][1])/T;
CMatrix H(2,4);//狀態->觀測
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<M;i++)
{
Filter_LMS();
for(j=0;j<350;j++)
{
ex[j]+=XY_Real[j][0]-XY_Filt[j][0];
ey[j]+=XY_Real[j][1]-XY_Filt[j][1];
dx[j]+=pow(XY_Real[j][0]-XY_Filt[j][0],2);
dy[j]+=pow(XY_Real[j][1]-XY_Filt[j][1],2);
}
}
for(j=0;j<350;j++)
{
ex[j]=ex[j]/M;
ey[j]=ey[j]/M;
dx[j]=sqrt(dx[j]/M-ex[j]);
dy[j]=sqrt(dy[j]/M-ey[j]);
}
for(j=0;j<350;j++)
{
cout<<ex[j]<<" "<<ey[j]<<endl;
cout<<dx[j]<<" "<<dy[j]<<endl;
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -