kf.cc

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

	{
		ballSeen=0;
		//cout<<"Ball being rejected "<<balldist<<endl<<flush;
	}
	//distance ball
	if (absBallDistance>550)
	{
		//cout<<"Invalid ball distance"<<balldist<<endl;
	}
	else
	{
		height=4;
		pixelError=4;
		pixelRelError=0.1;
		//R=pow(pixelError*(alpha*pow(beacondist,2)/height)+beacondist*pixelRelError,2)+pow((P[0][0]+P[1][1]+P[3][3]+P[4][4])/beacondist,2);
		//R=pow(pixelError*(alpha*pow(beacondist,2)/height)+beacondist*pixelRelError,2);
		if (balldist<0)
		{
			R=4*pow(visionConstant/(floor(visionConstant/absBallDistance+0.5)+4)-absBallDistance,2.0);
//Rick 23-6-04 added 1 line below: Extra uncertainty: Important when the ball is close
      R=R+10.0+0.04*absBallDistance*absBallDistance;
// Extra line for deadzone
      deadzone=5+0.1*absBallDistance;
		}
		else
		{
      R=pow(visionConstant/(floor(visionConstant/absBallDistance+0.5)+4)-absBallDistance,2.0);
//Rick 23-6-04 added 1 line below: Extra uncertainty: Important when the ball is close
      R=R+2.5+0.01*absBallDistance*absBallDistance;
// Extra line for deadzone
      deadzone=2+0.05*absBallDistance;
		}
		//cout<<"R Ball ="<<R<<endl<<flush;
		//cout<<"balldist="<<balldist<<endl<<flush;
		if (R < 0.0001)
			{
				R=0.0001;
			}
		hX=sqrt(pow(X[3][0]-X[0][0],2)+pow(X[4][0]-X[1][0],2));
    // changed from 0.1 by CM on Rick's advice.
		if(hX < 4){
			hX=4.0;
			//cout<<"hx is zero"<<endl<<flush;
		}

		C[0][0]=-(X[3][0]-X[0][0])/hX;
		C[0][1]=-(X[4][0]-X[1][0])/hX;
		C[0][2]=0;
		C[0][3]=(X[3][0]-X[0][0])/hX;
		C[0][4]=(X[4][0]-X[1][0])/hX;
		double innovation = absBallDistance-hX;
		cumError+=pow(innovation,2)/R;//((hx-Y))^2/R
    double CPC=convDble(C*P*C.transp());
    Deadzone(&R,&innovation,CPC,deadzone);

		J = P*C.transp()/(CPC+R);
		X = X+J*innovation;
		//X=X+J*(balldist-hX);
		P = (I-J*C)*P;
	}

	if(ABS(ballhead)< DEG_TO_RAD(140.0))//angle < ~120 degrees
	{	//ball angle update

		if (balldist<0)
		{
			R=0.04;
		}
		else
		{
			R=0.01;//+pow((P[0][0]+P[1][1]+P[3][3]+P[4][4]),2.0)/pow(balldist,4.0);
		}
		hX=Normalise_PI(atan2(X[4][0]-X[1][0],(X[3][0]-X[0][0]))-PI/2-X[2][0]);
		double denom=(pow(X[3][0]-X[0][0],2)+pow(X[4][0]-X[1][0],2));
		if (denom==0)
		{
			denom=0.0001;
		}
		C[0][0]=(X[4][0]-X[1][0])/denom;
		C[0][1]=-(X[3][0]-X[0][0])/denom;
		C[0][2]=-1;
		C[0][3]=-(X[4][0]-X[1][0])/denom;
		C[0][4]=(X[3][0]-X[0][0])/denom;
		double innovation=Normalise_PI(ballhead-hX);
    double CPC=convDble(C*P*C.transp());
    deadzone=DEG_TO_RAD(1.0);
    Deadzone(&R,&innovation,CPC,deadzone);

		cumError+=pow(innovation,2)/R;//((hx-Y)/R)^2
		J=P*C.transp()/(CPC+R);
		X=X+J*innovation;
		//X=X+J*(Normalise_PI(ballhead-hX));
		P=(I-J*C)*P;
		changeballx=X[3][0]-ballx;
		changebally=X[4][0]-bally;
	}
	else
	{
		cout<<"Chris you suck "<<ballhead<<endl<<flush;
	}

}


void KF::KFBallCoop(double ballx, double bally, double ballxuncert, double ballyuncert, double ballDistErrorPerFrame)
{

	    //Measurement update
	    static Matrix J(5,1),C(2,5),Y(2,1);
//    static const Matrix I(5,5, true);
	    static Matrix R(2,2);

	    Y[0][0]=ballx;
	    Y[1][0]=bally;

	    C[0][3]=1;
	    C[1][4]=1;
	    R[0][0]=pow(ballxuncert+10*ballDistErrorPerFrame,2);//ballxuncert calculated in BallPosition()
		//10*ballDistErrorPerFrame allowing for late packets
	    R[1][1]=pow(ballyuncert+10*ballDistErrorPerFrame,2);
		//double innovation=Y-C*X;
	    J=P*C.transp()*Invert22(C*P*C.transp()+R);
	    //X=X+J*innovation;
	    X=X+J*(Y-C*X);
	    P=(I-J*C)*P;
}


void KF::RobotPosnClip() {

  //If the Kalman Filter for the robot outputs a position that is not on the field, clip the position back into the
  //field in the direction that we have least confidence in.

  static Matrix v(5,1),w(5,1);

  //Condition 1: y<-210
  v[0][0]=0; v[1][0]=1; v[2][0]=0;//v={0, 1, 0, 0, 0};
  if (convDble(v.transp()*X)<MINYCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MINYCOORD)/(convDble(v.transp()*P*v));
  }
  //Condition 2: y>210
  v[0][0]=0; v[1][0]=1; v[2][0]=0;//v={0, 1, 0, 0, 0};etc
  if (convDble(v.transp()*X)>MAXYCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MAXYCOORD)/(convDble(v.transp()*P*v));
  }

  //Condition 3: x<-135
  v[0][0]=1; v[1][0]=0; v[2][0]=0;//v={1, 0, 0, 0, 0};
  if (convDble(v.transp()*X)<MINXCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MINXCOORD)/(convDble(v.transp()*P*v));
  }

  //Condition 4: x>135
  v[0][0]=1; v[1][0]=0; v[2][0]=0;//v={1, 0, 0 0, 0};
  if (convDble(v.transp()*X)>MAXXCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MAXXCOORD)/(convDble(v.transp()*P*v));
  }

  //Condition 5: x+y<-315 Bottom Left Hand Corner
  v[0][0]=1; v[1][0]=1; v[2][0]=0;//v={1, 1, 0, 0, 0};
  if (convDble(v.transp()*X)<-315) {
    X=X-P*v*(convDble(v.transp()*X)-(-315))/(convDble(v.transp()*P*v));
  }

  //Condition 6: -x+y<-315 Bottom Right Hand Corner
  v[0][0]=-1; v[1][0]=1; v[2][0]=0;//v={-1, 1, 0, 0, 0};
  if (convDble(v.transp()*X)<-315) {
    X=X-P*v*(convDble(v.transp()*X)-(-315))/(convDble(v.transp()*P*v));
  }

  //Condition 7: -x+y>315 Top Left Hand Corner
  v[0][0]=-1; v[1][0]=1; v[2][0]=0;//v={-1, 1, 0, 0, 0};
  if (convDble(v.transp()*X)>315) {
    X=X-P*v*(convDble(v.transp()*X)-315)/(convDble(v.transp()*P*v));
  }

  //Condition 8: x+y>315 Top Right Hand Corner
  v[0][0]=1; v[1][0]=1; v[2][0]=0;//v={1, 1, 0, 0, 0};
  if (convDble(v.transp()*X)>315) {
    X=X-P*v*(convDble(v.transp()*X)-315)/(convDble(v.transp()*P*v));
  }

  //Condition 9: y+7/15x>259 Top right adjacent goal
  v[0][0]=2.0/15.0; v[1][0]=1;//v={7/15,1,0,0,0};
  if (convDble(v.transp()*X)>224) {
    X=X-P*v*(convDble(v.transp()*X)-224)/(convDble(v.transp()*P*v));
  }

  //Condition 10: y+7/15x<-259 Bottom left adjacent goal
  v[0][0]=2.0/15.0; v[1][0]=1;//v={7/15,1,0,0,0};
  if (convDble(v.transp()*X)<-224) {
    X=X-P*v*(convDble(v.transp()*X)-(-224))/(convDble(v.transp()*P*v));
  }

  //Condition 11: -7/15x+y<-259 Bottom right adjacent goal
  v[0][0]=-2.0/15.0; v[1][0]=1;//v={-7/15,1,0,0,0};
  if (convDble(v.transp()*X)<-224) {
    X=X-P*v*(convDble(v.transp()*X)-(-224))/(convDble(v.transp()*P*v));
  }

  //Condition 12: -7/15x+y > 259 top left adjacent goal
  v[0][0]=-2.0/15.0; v[1][0]=1;//v={-7/15,1,0,0,0};
  if (convDble(v.transp()*X)>224) {
    X=X-P*v*(convDble(v.transp()*X)-224)/(convDble(v.transp()*P*v));
  }
}


//Function Resets uncertainties. Called if robot believes it is lost. Ensures faster reset.
void KF::Reset()
{
	cout<<"RESET "<<frame_<<endl<<flush;
	P[0][0]=18225;	P[1][1]=44100;	P[2][2]=40;	P[3][3]=18225;	P[4][4]=44100;
}


//calculated the bearing of an object to the robot
double KF::Bearing(double x, double y)
{
	return(Normalise_PI(atan2(y-X[1][0],x-X[0][0])-PI/2-X[2][0]));
}


//Checks for multiple objects seen in error.
void KF::CheckIfLost()
{
	double sum=0;
	double noObjects=0;
	for (int i=0; i<NUMVISIONTYPES; i++)
	{
		sum+=objectError[i];
		if(objectError[i]>0.3)
		{
			noObjects+=1;
		}
		objectError[i]*=0.90;
	}
	if (sum>3)
	{
		//cout<<"Sum greater than 3"<<sum<<endl<<flush;
	}
	if (noObjects>1)
	{
		//cout<<"Multiple Object in Error found "<<noObjects<<endl<<flush;
	}
	if ((sum>3)&&(noObjects>=3)) // changed from 2 to 4 by CM
	{
		Reset();
		for (int i=0; i<NUMVISIONTYPES; i++)
		{
			objectError[i]=0;
		}
	}

}


//Decides which goal corner has been seen
void KF::GoalCornerIn(int i, int* beaconSeen, double sdAngle)
{
	static int cornerCoords[4][2]={{50,160}, {-50,160},{-50,-160}, {50,-160}};//bl,br,yl,yr assuming you are standing IN respective goals facing out
  double hXleft, hXright;

	if(robotState_.GetTeam() == RobotState::RT_BLUE)//Blue team (Dog at blue goal end)
	{
		hXleft=Bearing(cornerCoords[0][0],cornerCoords[0][1]);
		hXright=Bearing(cornerCoords[1][0],cornerCoords[1][1]);
		if((beaconSeen[3])||(beaconSeen[4])||(beaconSeen[5]))//py,pg,pb beacons seen
		{
			if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<(15*PI/180))
			{
				//cout<<visionObjects_[i].heading_<<"Left blue corner decision"<<endl<<flush;
				KFSelfHeading(cornerCoords[0][0],cornerCoords[0][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			}
		}
		else if((beaconSeen[0])||(beaconSeen[1])||(beaconSeen[2]))//yp,gp,bp beacons seen
		{
			if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXright))<(15*PI/180))
			{
				//cout<<visionObjects_[i].heading_<<"Right blue corner decision"<<endl<<flush;
				KFSelfHeading(cornerCoords[1][0],cornerCoords[1][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			}

		}	else if(sqrt(P[2][2])<(10*PI/180))//<10degrees
		//if no beacons seen and angle uncertainty small enough, check estimate with actual
		{

			if (ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<ABS(Normalise_PI(visionObjects_[i].heading_ - hXright)))
			{
				if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<(10*PI/180))
				{
					//cout<<visionObjects_[i].heading_<<"Left blue corner decision"<<hXleft<<" "<<hXright<<endl<<flush;
					KFSelfHeading(cornerCoords[0][0],cornerCoords[0][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
				}
			}
			else if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXright))<(10*PI/180))
			{
				//cout<<"Right blue corner decision"<<endl<<flush;
				KFSelfHeading(cornerCoords[1][0],cornerCoords[1][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			}
		}
	} else if(robotState_.GetTeam() == RobotState::RT_RED)//Red team (dog at yellow end)
	{
		hXleft=Bearing(cornerCoords[2][0],cornerCoords[2][1]);
		hXright=Bearing(cornerCoords[3][0],cornerCoords[3][1]);
		if((beaconSeen[3])||(beaconSeen[4])||(beaconSeen[5]))//py,pg,pb beacons seen
		{
			if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXright))<(10*PI/180))
			{
				//cout<<"Right yellow corner decision"<<hXleft<<" "<<hXright<<endl<<flush;
				KFSelfHeading(cornerCoords[3][0],cornerCoords[3][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			}
		}	else if((beaconSeen[0])||(beaconSeen[1])||(beaconSeen[2])) { //yp,gp,bp beacons seen
			if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<(10*PI/180)) {
  			//cout<<"Left yellow Corner being used"<<endl<<flush;
				KFSelfHeading(cornerCoords[2][0],cornerCoords[2][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			}
		}	else if(sqrt(P[2][2])<(10*PI/180)) { //<10degrees
      //if no beacons seen and angle uncertainty small enough, check estimate with actual
			if (ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<ABS(Normalise_PI(visionObjects_[i].heading_ - hXright))) {
				if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<(10*PI/180)) {
					KFSelfHeading(cornerCoords[2][0],cornerCoords[2][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
				}
			} else if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXright))<(10*PI/180)) {
				KFSelfHeading(cornerCoords[3][0],cornerCoords[3][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			}
		}
	}
}


void KF::BallPosnClip() {
  static Matrix v(5,1),w(5,1);

  //Condition 1: y<-210
  v[3][0]=0; v[4][0]=1;//v={0,0,0,0,1};
  w[3][0]=1; w[4][0]=0;//w={0,0,0,1,0}
  if (convDble(v.transp()*X)<MINYCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MINYCOORD)/(convDble(v.transp()*P*v));
  }

  //Condition 2: y>210
  v[3][0]=0; v[4][0]=1;//v={0,0,0,0,1};
  if (convDble(v.transp()*X)>MAXYCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MAXYCOORD)/(convDble(v.transp()*P*v));
  }


  //Condition 3: x<-135
  v[3][0]=1; v[4][0]=0;//v={0,0,0,1,0};
  if (convDble(v.transp()*X)<MINXCOORD) {
	//    cout<<"x<-135"<<endl;
    X=X-P*v*(convDble(v.transp()*X)-MINXCOORD)/(convDble(v.transp()*P*v));
  }

  //Condition 4: x>135
  v[3][0]=1; v[4][0]=0;//v={0,0,0,1,0};
  if (convDble(v.transp()*X)>MAXXCOORD) {
    X=X-P*v*(convDble(v.transp()*X)-MAXXCOORD)/(convDble(v.transp()*P*v));
  }


  //Condition 5: x+y<-315 Bottom Left Hand Corner
  v[3][0]=1; v[4][0]=1;//v={0,0,0,1,1};
  if (convDble(v.transp()*X)<-315.0) {
	//    cout<<"x+y<-315"<<endl;
    X=X-P*v*(convDble(v.transp()*X)-(-315.0))/(convDble(v.transp()*P*v));
  }

  //Condition 6: -x+y<-315 Bottom Right Hand Corner
  v[3][0]=-1; v[4][0]=1;//v={0,0,0,-1,1};
	//    cout<<"-x+y<-315"<<endl;
  if (convDble(v.transp()*X)<-315.0) {
    X=X-P*v*(convDble(v.transp()*X)-(-315.0))/(convDble(v.transp()*P*v));
  }

  //Condition 7: -x+y>315 Top Left Hand Corner
  v[3][0]=-1; v[4][0]=1;//v={0,0,0,-1,1};
  if (convDble(v.transp()*X)>315.0) {
	//  cout<<"-x+y>315"<<endl;
    X=X-P*v*(convDble(v.transp()*X)-315.0)/(convDble(v.transp()*P*v));
  }

  //Condition 8: x+y>315 Top Right Hand Corner
  v[3][0]=1; v[4][0]=1;//v={0,0,0,1,1};
  if (convDble(v.transp()*X)>315.0) {
	//    cout<<"x+y>315"<<endl;
    X=X-P*v*(convDble(v.transp()*X)-315)/(convDble(v.transp()*P*v));
  }

  //Condition 9: y+7/15x>259 Top right adjacent goal
  v[3][0]=7.0/15.0; v[4][0]=1;//v={0,0,0,7/15,1};
  if (convDble(v.transp()*X)>259.0) {
    X=X-P*v*(convDble(v.transp()*X)-259.0)/(convDble(v.transp()*P*v));
  }

  //Condition 10: y+7/15x<-259 Borrom left adjacent goal
  v[3][0]=7.0/15.0; v[4][0]=1;//v={0,0,0,7/15,1};
  if (convDble(v.transp()*X)<-259.0) {
    X=X-P*v*(convDble(v.transp()*X)-(-259.0))/(convDble(v.transp()*P*v));
  }

  //Condition 11: 3y-x<-259 Bottom right adjacent goal
  v[3][0]=-7.0/15.0; v[4][0]=1;//v={0,0,0,-7/15,1};
  if (convDble(v.transp()*X)<-259.0) {
    X=X-P*v*(convDble(v.transp()*X)-(-259.0))/(convDble(v.transp()*P*v));
  }

  //Condition 12:3y-x>259 Top left adjacent goal
  v[3][0]=-7.0/15.0; v[4][0]=1;//v={0,0,0,-7/15,1};
  if (convDble(v.transp()*X)>259.0) {
    X=X-P*v*(convDble(v.transp()*X)-259.0)/(convDble(v.transp()*P*v));
  }
}


void KF::CentreLineCorner(int i, double sdAngle)
{
	double hXleft, hXright;
	hXleft=Bearing(-135,0);
	hXright=Bearing(135,0);
	if (ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<ABS(Normalise_PI(visionObjects_[i].heading_ - hXright)))
	{
		if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXleft))<(30*PI/180))
		{
			KFSelfHeading(-135,0,visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			//cout<<"Left centre edge point being used (gp)"<<endl<<flush;
		}
		else
		{
			//cout << "Left centre edge point being rejected" << endl << flush;
		}
	}
	else
	{
		if(ABS(Normalise_PI(visionObjects_[i].heading_ - hXright))<(30*PI/180))
		{
			KFSelfHeading(135,0,visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
			//cout<<"Right centre edge point being used (pg)"<<endl<<flush;
		}
		else
		{
			//cout<<"Right centre edge point being rejected"<<endl<<flush;
		}
	}
}