src/kalman_filter.cpp
//Common code for both of the functions
Common code for both of the functions
Common code for both of the functions
void KalmanFilter::CommonCode(const VectorXd &y){
  MatrixXd Ht = H_.transpose();
  MatrixXd S = H_*P_*Ht + R_;
  MatrixXd Si = S.inverse();
  MatrixXd K = P_*Ht*Si;
  x_ = x_ + K*y;
  int size = x_.size();
  MatrixXd I = MatrixXd::Identity(size, size);
  P_ = (I-K*H_)*P_;
}
void KalmanFilter :: CommonCode ( const VectorXd & y ) { MatrixXd Ht = H_ . transpose ( ) ; MatrixXd S = H_ * P_ * Ht + R_ ; MatrixXd Si = S . inverse ( ) ; MatrixXd K = P_ * Ht * Si ; x_ = x_ + K * y ; int size = x_ . size ( ) ; MatrixXd I = MatrixXd :: Identity ( size , size ) ; P_ = ( I - K * H_ ) * P_ ; }
Common code for both of the functions
Common code for both of the functions
//Define the Identity matrix