Driverless learning (III): Kalman filter

1、 Write Kalman filter

       Refer to the link below , Give comments on the basis of links , Write the Kalman filter as ：kalmanfilter.h

#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include "Eigen/Dense"

using namespace std;

class KalmanFilter{// Define a Kalman filter class 
public:
      //constructor: Is a method for creating and initializing “class Objects created ” Special method 
      KalmanFilter(){
           is_initalized_= false;
      };

      //destructor Destructor , Contrary to constructors ;
      // When an object ends its life cycle , When the function of the object has been called , The system automatically performs the destructor 
      //~KalmanFilter();

      void Initalization(Eigen::VectorXd x_in){
           x_= x_in;
           is_initalized_= true;
           //cout<<"intialized"<<endl;
      }

      bool IsInitalized(){
           return is_initalized_;
      }

     Eigen::VectorXd GetX(){
           return x_;
     }

     // State transition matrix F_（ Set function ）：state transistion matrix
     // You can give it first F_in assignment , And then call SetF() Function pair state transition matrix F_ Assign a value 
     void SetF(Eigen::MatrixXd F_in){
        F_ = F_in;
      }

     // State covariance matrix ( Set function )：state covariance matrix
     void SetP(Eigen::MatrixXd P_in){
        P_ = P_in;
      }

     // Process noise matrix ( Set function )：process covariance matrix
     void SetQ(Eigen::MatrixXd Q_in){

        Q_ = Q_in;
      }

     //x'= Fx + u. Next u Set to 0, That is, the external influence is ignored ( Simplify the model )
     //P' = F*P*Ft + Q
     void Prediction(){
        //cout<<"prediction"<<endl;
        x_ = F_ * x_;
        //cout<<" Predictive value  "<<x_<<endl;
        //cout<<" Predictive value F "<<F_<<endl;
        Eigen::MatrixXd Ft = F_.transpose();
        P_ = F_ * P_ * Ft + Q_;
     }

     // Measurement matrix :Measurement Matrix
     void SetH(Eigen::MatrixXd H_in){
        H_ = H_in;
      }

     // Measurement noise matrix :measurement covariance matrix
     void SetR(Eigen::MatrixXd R_in){
        R_ = R_in;
      }

     // forecast -- to update 
     // y=z-Hx'  Actual observations z And the forecast x' Difference between y
     // S=H*P*Ht+R S It's a temporary variable 
     // K=P'*Ht*S.inverse() K: Kalman gain , yes y The weight of the value 
     void Update(const Eigen::VectorXd &z){
        //  forecast 
        Eigen::VectorXd y = z - H_ * x_;
        Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
        Eigen::MatrixXd K = P_ * H_.transpose()*S.inverse(); // Kalman gain 

        // to update 
        x_ = x_+(K*y);// Update the current state vector 
        //cout<<" Update or get value  "<<x_<<endl;
        int size = x_.size();// Used to construct identity matrix 
        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
        P_ = (I - K*H_)*P_;// Update the uncertainty of the system 
     }       

// Defining parameters 
private:

      bool is_initalized_;// Whether to initialize 
      Eigen::VectorXd x_; //state vector State vector 
      Eigen::MatrixXd F_; // State transition matrix 
      Eigen::MatrixXd P_; // State covariance 
      Eigen::MatrixXd Q_; // noise 
      Eigen::MatrixXd H_; // Measurement matrix 
      Eigen::MatrixXd R_; // noise 

};


#endif

Reference link ：

Introduction to driverless technology （ 13、 ... and ）| Teach you to write Kalman filter by hand - You know