kf.h

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

#ifndef _KF_h_DEFINED
#define _KF_h_DEFINED

#include "../Globals.h"
#include "Matrix.h"

#define NUMVISIONTYPES 18


class KF {
  public:
    KF();
    void TimeUpdate(double distErrorStill, double angleErrorStill, double distErrorMoving, double angleErrorMoving, double ballDistError, double accel);
    void KFSelfDistance(int beaconx, int beacony,double beacondist, int index, double sdDistance);
    void KFSelfHeading(int beaconx, int beacony,double beaconhead,double beacondist,int index, double sdAngle);
    void KFTwoObjects(double markerdist, double markerangle,int beaconx, int beacony,int goalx, int goaly);
    void KFBallObject(int markerx, int markery, double markerangle, double ballangle);
    void KFBall(double ballhead, double balldist, int index);
    void KFBallCoop(double ballx, double bally, double ballxuncert, double ballyuncert, double ballDistErrorPerFrame);
    void GoalCornerIn(int i, int* beaconSeen, double sdAngle);
    void CentreLineCorner(int i, double sdAngle);
    void Deadzone(double* R, double* innovation, double CPC, double eps);
    void RobotPosnClip();
    void BallPosnClip();
    void Reset();
    void Restart();
    double Bearing(double x, double y);
    void CheckIfLost();

    Matrix X;
    Matrix P;
	  double cumError;

  private:
    Matrix I;
    double objectError[NUMVISIONTYPES];//number of different types of vision objects
    double changeballx, changebally, changerobotx,changeroboty;
    bool ballSeen;
};


#endif // _KF_h_DEFINED