3dpcp/.svn/pristine/58/58feec60fe6444a303bae3273f04bd42801f8b24.svn-base
2012-09-16 14:33:11 +02:00

278 lines
5.3 KiB
Text

/*
* kalmanfilter implementation
*
* Copyright (C) YuanJun, Li Wei, Li Ming, Andreas Nuechter,
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief
*
* @author Andreas Nuechter. Jacobs University Bremen, Germany
* @author YuanJun, Wuhan University, China
* @author Li Wei, Wuhan University, China
* @author Li Ming, Wuhan University, China
*/
/***************By yuanjun**************************/
#include "veloslam/kalmanfilter.h"
#include "math.h"
#define delta_t 0.1
KalmanFilter::KalmanFilter()
{
}
KalmanFilter::KalmanFilter(clusterFeature &glu,double rollAngle)
{
X1.SetMatrixRowAndCol(4,1);
X2.SetMatrixRowAndCol(4,1);
A.SetMatrixRowAndCol(4,4);
H.SetMatrixRowAndCol(2,4);
Z.SetMatrixRowAndCol(2,1);
K.SetMatrixRowAndCol(4,2);
Q.SetMatrixRowAndCol(4,4);
R.SetMatrixRowAndCol(2,2);
P1.SetMatrixRowAndCol(4,4);
P2.SetMatrixRowAndCol(4,4);
CoordinateRoll.SetMatrixRowAndCol(4,4);
X1.m_pTMatrix[0][0]=glu.avg_x;
X1.m_pTMatrix[1][0]=glu.avg_z;
X1.m_pTMatrix[2][0]=X1.m_pTMatrix[3][0]=25;
A.Eye();
A.m_pTMatrix[0][2]=delta_t;
A.m_pTMatrix[1][3]=delta_t;
H.m_pTMatrix[0][0]=1;
H.m_pTMatrix[1][1]=1;
Q.Eye();
R.Eye();
P1.Eye();
angle=rollAngle;
CalCoorRoll(angle);
}
KalmanFilter::~KalmanFilter(void)
{
}
void KalmanFilter::InitialKalmanFilter(clusterFeature &glu)
{
X1.SetMatrixRowAndCol(4,1);
X2.SetMatrixRowAndCol(4,1);
A.SetMatrixRowAndCol(4,4);
H.SetMatrixRowAndCol(2,4);
Z.SetMatrixRowAndCol(2,1);
K.SetMatrixRowAndCol(4,2);
Q.SetMatrixRowAndCol(4,4);
R.SetMatrixRowAndCol(2,2);
P1.SetMatrixRowAndCol(4,4);
P2.SetMatrixRowAndCol(4,4);
X1.m_pTMatrix[0][0]=glu.avg_x;
X1.m_pTMatrix[1][0]=glu.avg_z;
X1.m_pTMatrix[2][0]=X1.m_pTMatrix[3][0]=1;
A.Eye();
A.m_pTMatrix[0][2]=delta_t;
A.m_pTMatrix[1][3]=delta_t;
H.m_pTMatrix[0][0]=1;
H.m_pTMatrix[1][1]=1;
Q.Eye();
R.Eye();
P1.Eye();
}
void KalmanFilter::timeUpdate()
{
CMatrix T1,T2;
X2=A*X1;
T1= A.Transpose();
P2= A * P1 * T1 + Q;
// X2=A*X1;
// P2=A*P1*A.Transpose()+Q;
}
void KalmanFilter::stateUpdate(clusterFeature &glu,double rollangle,double *pos)
{
CMatrix T1,T2;
//Z.m_pTMatrix[0][0]=glu.avg_x;
//Z.m_pTMatrix[1][0]=glu.avg_z;
//CMatrix temp(2,2);
// T1= H.Transpose();
//temp=H*P2*T1+R;
// T2= temp.Inverse();
//K=P2*T1*T2;
//CalCoorRoll(rollangle);
//X2=CoordinateRoll*X2;
// CMatrix T3,T4,T5;
// T4=H*X2;
// T3=Z-T4;
// T5=K*T3;
//X1=X2+T5;
//CMatrix I(4,4);
//I.Eye();
// CMatrix T6,T7,T8;
// T6=K*H;
// T7=I-T6;
//P1=T7*P2;
Z.m_pTMatrix[0][0]=glu.avg_x;
Z.m_pTMatrix[1][0]=glu.avg_z;
// CMatrix temp(2,2);
// temp=H*P2*H.Transpose()+R;
CMatrix temp(2,2);
T1= H.Transpose();
temp=H*P2*T1+R;
T2= temp.Inverse();
K=P2*T1*T2;
// K=P2*H.Transpose()*temp.Inverse();
CalCoorRoll(rollangle);
X2=CoordinateRoll*X2;
X2.m_pTMatrix[0][0]+=pos[0];
X2.m_pTMatrix[1][0]+=pos[2];
// X1=X2+K*(Z-H*X2);
CMatrix T3,T4,T5;
T4=H*X2;
T3=Z-T4;
T5=K*T3;
X1=X2+T5;
CMatrix I(4,4);
I.Eye();
// P1=(I-K*H)*P2;
CMatrix T6,T7,T8;
T6=K*H;
T7=I-T6;
P1=T7*P2;
}
ObjectState KalmanFilter::GetCurrentState()
{
ObjectState movestate;
movestate.x_position=X1.m_pTMatrix[0][0];
movestate.z_positon=X1.m_pTMatrix[1][0];
movestate.x_speed=X1.m_pTMatrix[2][0];
movestate.z_speed=X1.m_pTMatrix[3][0];
return movestate;
}
ObjectState KalmanFilter::GetPredictState()
{
ObjectState movestate;
movestate.x_position=X2.m_pTMatrix[0][0];
movestate.z_positon=X2.m_pTMatrix[1][0];
movestate.x_speed=X2.m_pTMatrix[2][0];
movestate.z_speed=X2.m_pTMatrix[3][0];
return movestate;
}
Measurement KalmanFilter::GetPredictMeasurement(double rollAngle,double *pos)
{
Measurement predictMeasurement;
CMatrix temp(2,1);
CalCoorRoll(rollAngle);
X2=CoordinateRoll*X2;
X2.m_pTMatrix[0][0]+=pos[0];
X2.m_pTMatrix[1][0]+=pos[2];
temp=H*X2;
predictMeasurement.x_measurement=temp.m_pTMatrix[0][0];
predictMeasurement.z_measurement=temp.m_pTMatrix[1][0];
return predictMeasurement;
}
CMatrix KalmanFilter::CalMeasureDeviation()
{
CMatrix Deviation(2,2),standardDeviation(2,2);
CMatrix T6,T7,T8;
T6=H.Transpose();
T7=P2*T6;
Deviation=H*T7+R;
// Deviation=H*P2*H.Transpose()+R;
for (int i=0;i<2;i++)
{
for(int j=0;j<2;j++)
{
standardDeviation.m_pTMatrix[i][j]=sqrt(Deviation.m_pTMatrix[i][j]);
}
}
return standardDeviation;
}
KalmanFilter& KalmanFilter::operator = (const KalmanFilter& anotherKF)
{
A=anotherKF.A;
H=anotherKF.H;
X1=anotherKF.X1;
X2=anotherKF.X2;
K=anotherKF.K;
Z=anotherKF.Z;
P1=anotherKF.P1;
P2=anotherKF.P2;
Q=anotherKF.Q;
R=anotherKF.R;
CoordinateRoll=anotherKF.CoordinateRoll;
angle=anotherKF.angle;
return *this;
}
void KalmanFilter::CalCoorRoll(double angle)
{
CoordinateRoll.m_pTMatrix[0][0]=cos(angle);//angle is radian??
CoordinateRoll.m_pTMatrix[0][1]=sin(angle);
CoordinateRoll.m_pTMatrix[1][0]=-sin(angle);
CoordinateRoll.m_pTMatrix[1][1]=cos(angle);
CoordinateRoll.m_pTMatrix[2][2]=cos(angle);
CoordinateRoll.m_pTMatrix[2][3]=sin(angle);
CoordinateRoll.m_pTMatrix[3][2]=-sin(angle);
CoordinateRoll.m_pTMatrix[3][3]=cos(angle);
}