修改Xd为Xf因为需要用float，不能使用double

tracker/kalman_filter.cpp

@@ -6,27 +6,27 @@ KalmanFilter::KalmanFilter(unsigned int num_states, unsigned int num_obs) :
         num_states_(num_states), num_obs_(num_obs) {
     /*** Predict ***/
     // State vector
-    x_ = Eigen::VectorXd::Zero(num_states);
+    x_ = Eigen::VectorXf::Zero(num_states);
     // Predicted(a prior) state vector
-    x_predict_ = Eigen::VectorXd::Zero(num_states);
+    x_predict_ = Eigen::VectorXf::Zero(num_states);
 
     // State transition matrix F_
-    F_ = Eigen::MatrixXd::Zero(num_states, num_states);
+    F_ = Eigen::MatrixXf::Zero(num_states, num_states);
 
     // Error covariance matrix P
-    P_ = Eigen::MatrixXd::Zero(num_states, num_states);
+    P_ = Eigen::MatrixXf::Zero(num_states, num_states);
     // Predicted(a prior) error covariance matrix
-    P_predict_ = Eigen::MatrixXd::Zero(num_states, num_states);
+    P_predict_ = Eigen::MatrixXf::Zero(num_states, num_states);
 
     // Covariance matrix of process noise
-    Q_ = Eigen::MatrixXd::Zero(num_states, num_states);
+    Q_ = Eigen::MatrixXf::Zero(num_states, num_states);
 
     /*** Update ***/
     // Observation matrix
-    H_ = Eigen::MatrixXd::Zero(num_obs, num_states);
+    H_ = Eigen::MatrixXf::Zero(num_obs, num_states);
 
     // Covariance matrix of observation noise
-    R_ = Eigen::MatrixXd::Zero(num_obs, num_obs);
+    R_ = Eigen::MatrixXf::Zero(num_obs, num_obs);
 
     log_likelihood_delta_ = 0.0;
     NIS_ = 0.0;
@@ -46,21 +46,21 @@ void KalmanFilter::Predict() {
 }
 
 
-Eigen::VectorXd KalmanFilter::PredictionToObservation(const Eigen::VectorXd &state) {
+Eigen::VectorXf KalmanFilter::PredictionToObservation(const Eigen::VectorXf &state) {
     return (H_*state);
 }
 
 
-void KalmanFilter::Update(const Eigen::VectorXd& z) {
-    Eigen::VectorXd z_predict = PredictionToObservation(x_predict_);
+void KalmanFilter::Update(const Eigen::VectorXf& z) {
+    Eigen::VectorXf z_predict = PredictionToObservation(x_predict_);
 
     // y - innovation, z - real observation, z_predict - predicted observation
-    Eigen::VectorXd y = z - z_predict;
+    Eigen::VectorXf y = z - z_predict;
 
-    Eigen::MatrixXd Ht = H_.transpose();
+    Eigen::MatrixXf Ht = H_.transpose();
 
     // S - innovation covariance
-    Eigen::MatrixXd S = H_ * P_predict_ * Ht + R_;
+    Eigen::MatrixXf S = H_ * P_predict_ * Ht + R_;
 
     NIS_ = y.transpose() * S.inverse() * y;
 
@@ -85,12 +85,12 @@ void KalmanFilter::Update(const Eigen::VectorXd& z) {
 
 
     // K - Kalman gain
-    Eigen::MatrixXd K = P_predict_ * Ht * S.inverse();
+    Eigen::MatrixXf K = P_predict_ * Ht * S.inverse();
 
     // Updated state estimation
     x_ = x_predict_ + K * y;
 
-    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(num_states_, num_states_);
+    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(num_states_, num_states_);
     // Joseph form
     //P_ = (I - K * H_) * P_predict_ * (I - K * H_).transpose() + K * R_ * K.transpose();
     // Optimal gain
@@ -98,7 +98,7 @@ void KalmanFilter::Update(const Eigen::VectorXd& z) {
 }
 
 
-float KalmanFilter::CalculateLogLikelihood(const Eigen::VectorXd& y, const Eigen::MatrixXd& S) {
+float KalmanFilter::CalculateLogLikelihood(const Eigen::VectorXf& y, const Eigen::MatrixXf& S) {
     float log_likelihood;
 
     // Note: Computing log(M.determinant()) in Eigen C++ is risky for large matrices since it may overflow or underflow.

tracker/kalman_filter.h

@@ -37,36 +37,36 @@ public:
      * using the observation model
      * User can implement their own method for more complicated models
      */
-    virtual Eigen::VectorXd PredictionToObservation(const Eigen::VectorXd &state);
+    virtual Eigen::VectorXf PredictionToObservation(const Eigen::VectorXf &state);
 
     /**
      * Updates the state by using Extended Kalman Filter equations
      * @param z The measurement at k+1
      */
-    virtual void Update(const Eigen::VectorXd &z);
+    virtual void Update(const Eigen::VectorXf &z);
 
     /**
      * Calculate marginal log-likelihood to evaluate different parameter choices
      */
-    float CalculateLogLikelihood(const Eigen::VectorXd& y, const Eigen::MatrixXd& S);
+    float CalculateLogLikelihood(const Eigen::VectorXf& y, const Eigen::MatrixXf& S);
 
     // State vector
-    Eigen::VectorXd x_, x_predict_;
+    Eigen::VectorXf x_, x_predict_;
 
     // Error covariance matrix
-    Eigen::MatrixXd P_, P_predict_;
+    Eigen::MatrixXf P_, P_predict_;
 
     // State transition matrix
-    Eigen::MatrixXd F_;
+    Eigen::MatrixXf F_;
 
     // Covariance matrix of process noise
-    Eigen::MatrixXd Q_;
+    Eigen::MatrixXf Q_;
 
     // measurement matrix
-    Eigen::MatrixXd H_;
+    Eigen::MatrixXf H_;
 
     // covariance matrix of observation noise
-    Eigen::MatrixXd R_;
+    Eigen::MatrixXf R_;
 
     unsigned int num_states_, num_obs_;