kf.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "KF.h"
#include "../Globals.h"
#include "../Common/VisionObject.h"
#include "../Common/WorldObject.h"
#include "../Common/Common.h"
#include "../Common/RobotState.h"
#include <math.h>

using namespace std;

#define MINXCOORD -135.0
#define MAXXCOORD 135.0

// old. changed by CM. suggestion from Rick !
/*
#define MINYCOORD -245.0
#define MAXYCOORD 245.0
*/

#define MINYCOORD -220.0
#define MAXYCOORD 220.0


KF::KF()
{
  I=Matrix(5,5,true);
  P=Matrix(5,5);
  X=Matrix(5,1);

  Restart(); // init!
}

void KF::Restart()
{
  int i = 0;
  for (i=0; i<5; i++) {
    for (int j=0; j<5; j++) {
      P[i][j]=0;
      X[i][0]=0;
    }
  }

	P[0][0]=18225;	P[1][1]=44100;	P[2][2]=40;	P[3][3]=18225;	P[4][4]=44100;
	X[0][0]=-0.001; X[1][0]=-0.001; X[2][0]=0; X[3][0]=0.001;	X[4][0]=0.001;
	cumError=0;
	changeballx=0;
	changebally=0;
	changerobotx=0;
  changeroboty=0;

  for (i=0; i<NUMVISIONTYPES; i++) {
		objectError[i]=0;
	}

	ballSeen=false;

}


void KF::TimeUpdate(double distErrorStill, double angleErrorStill, double distErrorMoving, double angleErrorMoving, double ballDistError, double accel) {
	//time update
  static Matrix Q(5,5), u(5,1);
	static double TheStrutUpdate=1.0;
  double distErrorPerFrame,angleErrorPerFrame;

  // odometry calculations. notice the reliance on robot heading here ...
  // possibly can account for this by increasing Q
  LocomotionData ld = lcq_.GetLocomotionData();
	u[0][0]=-ld.deltaForward*sin(X[2][0])/TheStrutUpdate-ld.deltaLeft*cos(X[2][0])/TheStrutUpdate;
	u[1][0]=ld.deltaForward*cos(X[2][0])/TheStrutUpdate-ld.deltaLeft*sin(X[2][0])/TheStrutUpdate;
	u[2][0]=ld.deltaTurn/TheStrutUpdate;

  // ball accelerations = 0.
  u[3][0]=0;
	u[4][0]=0;

  // changing these parameters changes balances between odometry and measurements
  // larger values => larger uncertainties in odometry
  if((u[0][0]==0)&&(u[1][0]==0)&&(u[2][0]==0)) {
    // should be approx error per frame in odometry, incl hits/moves, etc.
    distErrorPerFrame = distErrorStill;
    angleErrorPerFrame = angleErrorStill;
  } else {
    distErrorPerFrame = distErrorMoving;
	  angleErrorPerFrame = angleErrorMoving;
  }
  //cout<<ballDistError<<"ballDistError"<<endl<<flush;
  //Q values chosen to linearise sqrt(P). tuning parameters.
  Q[0][0]=2*distErrorPerFrame*sqrt(P[0][0]) + pow(distErrorPerFrame,2);
  Q[1][1]=2*distErrorPerFrame*sqrt(P[1][1]) + pow(distErrorPerFrame,2);
  Q[2][2]=2*angleErrorPerFrame*sqrt(P[2][2]) + pow(angleErrorPerFrame,2);
  Q[3][3]=2*ballDistError*sqrt(P[3][3]) + pow(ballDistError,2);
  Q[4][4]=2*ballDistError*sqrt(P[4][4]) + pow(ballDistError,2);
  //time update
  X = X + u;
	P = P + Q;

}


//uses beacons and goal posts to localise self
void KF::KFSelfDistance(int beaconx, int beacony,double beacondist, int index,double sdDistance)
{
	static Matrix C(1,5),J(5,1);
//static const Matrix I(5,5,true);
	static double hX,R;
	//static double visionConstant=1800.0;
	Matrix Xcheck(5,1);
	double alpha=0.3/180*PI;
	double height, deadzone;
	int pixelError;
	double pixelRelError;
	double x,y;
	x=X[0][0];
	y=X[1][0];


	if ((beacondist>550)||(beacondist<0))
	{
		//cout<<"Invalid distance"<<beacondist<<endl;
	}
	else
	{
    //Ricks calc for the deadzone variable
    deadzone=5.0+0.05*beacondist;
		//distance self
		if ((visionObjects_[index].type_>=VisionObject::OT_BEACON_YP)&&(visionObjects_[index].type_<=VisionObject::OT_BEACON_PB))
		{
			height=10;
			pixelError=2; //RHM 23-6-04 pixelError=1;
			pixelRelError=0.1;
			R=pow(pixelError*(alpha*pow(beacondist,2)/height)+beacondist*pixelRelError,2)+pow((P[0][0]+P[1][1])/beacondist,2);
		}
		else
		//goal uncertainty higher
		{
      //Double the deadzone for goal range
      deadzone=2.0*deadzone;

			height=10;
			pixelError=6;
			pixelRelError=0.1;
			/*if (beacondist<110)
			{
				//R=pow(30+ pixelError*(alpha*pow(beacondist,2)/height)+beacondist*pixelRelError,2)+pow((P[0][0]+P[1][1])/beacondist,2);
				R=pow(pixelError*(alpha*pow(beacondist,2)/height)+beacondist*pixelRelError+30,2);
			}*/
			//else
			{
				R=pow(pixelError*(alpha*pow(beacondist,2)/height)+beacondist*pixelRelError,2)+pow((P[0][0]+P[1][1])/beacondist,2);
			}
		}

		hX=sqrt(pow((X[0][0]-beaconx),2)+pow((X[1][0]-beacony),2));
		C[0][0]=(X[0][0]-beaconx)/sqrt(pow(X[0][0]-beaconx,2)+pow(X[1][0]-beacony,2));
		C[0][1]=(X[1][0]-beacony)/sqrt(pow(X[0][0]-beaconx,2)+pow(X[1][0]-beacony,2));
		C[0][2]=0; C[0][3]=0; C[0][4]=0;
		
    double CPC=convDble(C*P*C.transp());
		double innovation=beacondist-hX;
    //Ricks Deadzone implementation
    Deadzone(&R,&innovation,CPC,deadzone);    

    double sdPredErr=CPC+R;
		J=P*C.transp()/sdPredErr;


		//cumError+=pow(innovation,2)/R;//((hx-Y)/R)^2
		if(ABS(innovation)>sdDistance*sqrt(sdPredErr))
		{
			//cout<<frame_<<" Dud measurement distance "<<visionObjects_[index].type_<<" "<<innovation<<endl<<flush;
			objectError[visionObjects_[index].type_]+=1;
		}
		else//perform measurement update
		{
			X=X+J*innovation;
			P=(I-J*C)*P;
		}
	}
	changerobotx+=(X[0][0]-x);
	changeroboty+=(X[1][0]-y);

}
void KF::KFSelfHeading(int beaconx, int beacony,double beaconhead,double beacondist,int index, double sdAngle)
{
	static Matrix C(1,5),J(5,1);
//static const Matrix I(5,5,true);
	static double hX,R,deadzone;
	//static double visionConstant=1800;
	double x,y;
	x=X[0][0];
	y=X[1][0];
	//Check added to prevent divide by zero (CS - 2003/06/30)
	if (ABS(beacondist) < 1) {
		beacondist = 1;
    }
    //angle self

// reduction in R => more certain of measurements

    if ((visionObjects_[index].type_>=VisionObject::OT_BEACON_YP)&&(visionObjects_[index].type_<=VisionObject::OT_BEACON_PB))
	{
    // ALL BEACONS
		R=0.002/*+pow(P[0][0]+P[1][1],2)/pow(beacondist,4)*/;
    deadzone=DEG_TO_RAD(1.0);
    
	}
	else if (visionObjects_[index].type_==VisionObject::OT_GOAL_CORNER_IN)
	{
		R=0.02;//+pow(P[0][0]+P[1][1],2)/pow(beacondist,4);
    deadzone=DEG_TO_RAD(2.0);
	}
	else if (visionObjects_[index].type_==VisionObject::OT_CENTRE_POINT)
	{
		R=0.02;//+pow(P[0][0]+P[1][1],2)/pow(beacondist,4);
    deadzone=DEG_TO_RAD(2.0);
	}
	else if (visionObjects_[index].type_==VisionObject::OT_CENTRE_LINE_CORNER)
	{
		R=0.02;//+pow(P[0][0]+P[1][1],2)/pow(beacondist,4);
    deadzone=DEG_TO_RAD(2.0);
	}
	else
	{
    // GOAL POSTS
		R=0.005/*+pow(P[0][0]+P[1][1],2)/pow(beacondist,4)*/; //0.01
    deadzone=DEG_TO_RAD(2.0);
	}

	hX=Normalise_PI(atan2(beacony-X[1][0],beaconx-X[0][0])-PI/2-X[2][0]);
    C[0][0]=(beacony-X[1][0])/(pow(X[0][0]-beaconx,2)+pow(X[1][0]-beacony,2));
	C[0][1]=(X[0][0]-beaconx)/(pow(X[0][0]-beaconx,2)+pow(X[1][0]-beacony,2));
	C[0][2]=-1; C[0][3]=0; C[0][4]=0;
	double innovation=Normalise_PI(beaconhead-hX);
	//cumError+=pow(innovation,2)/R;//((hx-Y)/R)^2

// Ricks extra for deadzone:
  double CPC=convDble(C*P*C.transp());
  Deadzone(&R,&innovation,CPC,deadzone);
	double sdPredErr=CPC+R;


	J=P*C.transp()/(CPC+R);
  // if ((measurement-prediction) > some number * std dev) then outlier.
	if(ABS(innovation)>sdAngle*sqrt(sdPredErr))
	{
		//cout<<frame_<<" Dud angle measurement "<<innovation<<endl<<flush;
		objectError[visionObjects_[index].type_]+=1;
	}
	else
	{
		X=X+J*innovation;
 		P=(I-J*C)*P;
	}
	changerobotx+=(X[0][0]-x);
	changeroboty+=(X[1][0]-y);
}

//Ricks hack for a deadzone update
void KF::Deadzone(double* R, double* innovation, double CPC, double eps)
{
double invR;
// R is the covariance of the measurement (altered by this procedure)
// innovation is the prediction error (altered by this procedure)
// CPC is the variance of the predicted y value (not altered)
// eps is the value of the deadzone
// all vars except innovation must e positive for this to work

//Return if eps or CPC or R non-positive
if ((eps<1.0e-06) || (CPC<1.0e-06) || (*R<1e-06))
  return;

if (ABS(*innovation)>eps)
  return;

// If we get to here, then valid parameters passed, and we
// are within the specified accuracy
*innovation=0.0; //Set to zero to force no update of parameters

invR=0.25/(eps*eps)-1.0/CPC;
if (invR<1.0e-06)
  invR=1e-06;

// only ever increase R
if ( *R < 1.0/invR )
  *R=1.0/invR;
// R is adjusted so that the a-posteriori variance of the 
// prediction is 4*eps*eps
}




//uses angle between two objects
void KF::KFTwoObjects(double markerdist, double markerangle,int beaconx, int beacony,int goalx, int goaly)
{
	static Matrix C(1,5),J(5,1);
//static const Matrix I(5,5,true);
	static double hX,R;
	double a2,dadx,dady,b2,dbdx,dbdy,costheta,dcosthetadx,dcosthetady;

	R=0.003;//+pow(P[0][0]+P[1][1],2)/pow(beacondist,4);
	a2=pow((X[0][0]-beaconx),2)+pow((X[1][0]-beacony),2); //a squared
	dadx=(X[0][0]-beaconx)/sqrt(a2);
	dady=(X[1][0]-beacony)/sqrt(a2);
	b2=pow(X[0][0]-goalx,2)+pow(X[1][0]-goaly,2); //b squared
	dbdx=(X[0][0]-goalx)/sqrt(b2);
	dbdy=(X[1][0]-goaly)/sqrt(b2);
	costheta=(a2+b2-pow(markerdist,2))/(2*sqrt(a2*b2));
	dcosthetadx=(1/(2*sqrt(a2*b2)))*(2*sqrt(b2)*dbdx+2*sqrt(a2)*dadx-(b2+a2-pow(markerdist,2))*(dbdx/sqrt(b2)-dadx/sqrt(a2)));
	dcosthetady=(1/(2*sqrt(a2*b2)))*(2*sqrt(b2)*dbdy+2*sqrt(a2)*dady-(b2+a2-pow(markerdist,2))*(dbdy/sqrt(b2)-dady/sqrt(a2)));

	hX=Normalise_PI(acos(costheta));
  if (hX < DEG_TO_RAD(15)) {
    return;
  }
	C[0][0]=-dcosthetadx/(sqrt(1-pow(costheta,2)));
	C[0][1]=-dcosthetady/(sqrt(1-pow(costheta,2)));
	C[0][2]=0;
	C[0][3]=0;
	C[0][4]=0;
	double sdPredErr=convDble(C*P*C.transp())+R;
	double innovation=Normalise_PI(markerangle-hX);
	//cumError+=pow(innovation,2)/R;//((hx-Y)/R)^2

	if(sqrt(sdPredErr)>0.2)
	{
		//cout<<"SD for prediction error angle"<<sqrt(sdPredErr)<<endl<<flush;
		sdPredErr=0.04;//max value to ensure measurements aren't ignored when P large
	}
// CHANGE BY CM -- CHECK ME
/*
	if(ABS(innovation)>sdAngle*sqrt(sdPredErr))
	{
		//cout<<frame_<<" Dud ae measurement "<<innovation<<endl<<flush;
		objectError[visionObjects_[index].type_]+=1;
	}
	else
	{
		X=X+J*innovation;
 		P=(I-J*C)*P;
	}
*/
/*f(ABS(innovation) > 0.8*sqrt(sdPredErr))
	{
//    cout << "innov: " << ABS(innovation) << ", 0.5: " << 0.5*sqrt(sdPredErr) << endl << flush;
		cout<<"two beacon measurement ignored"<<endl<<flush;
    return;
	}
*/
	J=P*C.transp()/(convDble(C*P*C.transp())+R);
	X=X+J*(innovation);
  P=(I-J*C)*P;
}

void KF::KFBallObject(int markerx, int markery, double markerangle, double ballangle)
{
	static Matrix C(1,5),J(5,1);
//static const Matrix I(5,5,true);
	static double hX,R;
	double a2,dadx,dady,dadbx,dadby,b2,dbdx,dbdy,dbdbx,dbdby,c2,dcdx,dcdy,dcdbx,dcdby,costheta,dcosthetadx,dcosthetady,dcosthetadbx,dcosthetadby;
	double angle=ABS(markerangle-ballangle);
	R=0.04;//+pow(P[0][0]+P[1][1],2)/pow(beacondist,4);
	a2=pow((markerx-X[0][0]),2)+pow((markery-X[1][0]),2);
	dadx=(X[0][0]-markerx)/sqrt(a2);
	dady=(X[1][0]-markery)/sqrt(a2);
	dadbx=0;
	dadby=0;

	b2=pow(X[3][0]-X[0][0],2)+pow(X[4][0]-X[1][0],2);
	dbdx=(X[0][0]-X[3][0])/sqrt(b2);
	dbdy=(X[1][0]-X[4][0])/sqrt(b2);
	dbdbx=(X[3][0]-X[0][0])/sqrt(b2);
	dbdby=(X[4][0]-X[1][0])/sqrt(b2);

	c2=pow(markerx-X[3][0],2)+pow(markery-X[4][0],2);
	dcdx=0;
	dcdy=0;
	dcdbx=-(markerx-X[3][0])/sqrt(c2);
	dcdby=-(markery-X[4][0])/sqrt(c2);

	costheta=(a2+b2-c2)/(2*sqrt(a2*b2));
	dcosthetadx=(1/(2*sqrt(b2)*sqrt(a2)))*((2*sqrt(a2)*dadx+2*sqrt(b2)*dbdx-2*sqrt(c2)*dcdx)-0.5*(a2+b2-c2)*(dbdx/sqrt(b2)+dadx/sqrt(a2)));
	dcosthetady=(1/(2*sqrt(b2)*sqrt(a2)))*((2*sqrt(a2)*dady+2*sqrt(b2)*dbdy-2*sqrt(c2)*dcdy)-0.5*(a2+b2-c2)*(dbdy/sqrt(b2)+dady/sqrt(a2)));
	dcosthetadbx=(1/(2*sqrt(b2)*sqrt(a2)))*((2*sqrt(a2)*dadbx+2*sqrt(b2)*dbdbx-2*sqrt(c2)*dcdbx)-0.5*(a2+b2-c2)*(dbdbx/sqrt(b2)+dadbx/sqrt(a2)));
	dcosthetadby=(1/(2*sqrt(b2)*sqrt(a2)))*((2*sqrt(a2)*dadby+2*sqrt(b2)*dbdby-2*sqrt(c2)*dcdby)-0.5*(a2+b2-c2)*(dbdby/sqrt(b2)+dadby/sqrt(a2)));

	hX=Normalise_PI(acos(costheta));
  double denom = (sqrt(1-pow(costheta,2)));
	C[0][0]=-dcosthetadx/denom;
	C[0][1]=-dcosthetady/denom;
	C[0][2]=0;
	C[0][3]=-dcosthetadbx/denom;
	C[0][4]=-dcosthetadby/denom;


//not used now - CM
//double sdPredErr=convDble(C*P*C.transp())+R;

	double innovation=Normalise_PI(angle-hX);
	//cumError+=pow(innovation,2)/R;//((hx-Y)/R)^2

//why not use ?
//sdPredErr=MAX(sdPredErr,0.04);
/* CHANGE BY CM, CHECK ME
	if(sqrt(sdPredErr)>0.2)
	{
		//cout<<"SD for prediction error angle"<<sqrt(sdPredErr)<<endl<<flush;
		sdPredErr=0.04;//max value to ensure measurements aren't ignored when P large
	}
	if(pow(innovation,2)<0.4*sdPredErr)
	{
		//cout<<"measurement ignored "<<innovation<<endl<<flush;
		innovation=0;
		//possibly R made large instead???
	}*/
	J=P*C.transp()/(convDble(C*P*C.transp())+R);
   	X=X+J*(innovation);
    P=(I-J*C)*P;
}


void KF::KFBall(double ballhead, double balldist,int index)
{
	static Matrix C(1,5),J(5,1),Ctrans(5,1),Xcheck(5,1);
//static const Matrix I(5,5,true);
	static double hX,R;
	static double visionConstant=600.0;
	//double alpha=0.3/180*PI;
	double height;
	int pixelError;
	double pixelRelError, deadzone;
	double ballx, bally;
  double absBallDistance = ABS(balldist);
	ballx=X[3][0];
	bally=X[4][0];
	if ((absBallDistance<550)&&(absBallDistance>50))
	{
		if(ballSeen>0)
		{
			ballSeen=2;
		}
		else
		{
			ballSeen=1;
		}
	}
	else