今天用c++ 写了 一个卡尔曼的小样，比较简单，分享给大家，后期有可能也会修改。 以后也会慢慢的把其他的 自己写的代码分享给大家。 本人还是个 小白，写的不好轻喷。
因为传入的参数都比较简单，所以测量和观测矩阵都是用的单位矩阵
这个是在ubuntu 下用Qt写的，不过是用cmake编译的，中间用了eigen这个库，第一次用，还是不太熟悉
main.cpp #include "kalman.h" class KalmanFilter; int main(int argc, char *argv[]) { double ran_num; Eigen::Matrix<double, 4, 1> test1 ; Eigen::Matrix<double, 4, 1> test2 ; KalmanFilter kf(2,2); while(1){ for(int i =0 ; i<4 ; i++){ ran_num = rand()%9+100; test1(i,0) =(double) ran_num; } test2 = kf.updata_paramount(test1); for( int j =0; j<4 ;j++){ std::cout<< test2(j,0) <<" "<< test1(j,0)<< std::endl ; } } return 0; } kalman.h #ifndef KALMAN_H #define KALMAN_H #include <Eigen/Core> #include <Eigen/LU> #include <Eigen/Eigen> #include <Eigen/Dense> #include <ctime> #include <iostream> class KalmanFilter{ public: KalmanFilter(); KalmanFilter( const double ProcessNoise_Q ,const double MeasureNoise_R); ~KalmanFilter(); Eigen::Matrix<double,4,1> updata_paramount(Eigen::Matrix<double, 4, 1> Data); protected: Eigen::Matrix<double,4,1> measure_Z; double Noise_Q; //Systerm noise double Noise_R; //Measurement noise private: static Eigen::Matrix<double,4,1> X_last; //x(k-1|k-1) Eigen::Matrix<double,4,1> X_pre; //x(k|k-1) Eigen::Matrix<double,4,1> X_now; //x(k|k) static Eigen::Matrix<double,4,4> P_last ; //p(k-1|k-1)getrow Eigen::Matrix<double,4,4> P_pre; //p(k|k-1) Eigen::Matrix<double,4,4> P_update; //p(k|k) Eigen::Matrix<double,4,4> Kg; //kg(k) Eigen::Matrix<double,4,4> trans_A; //matrix_A Eigen::Matrix<double,4,4> meas_H; //mattix_H Eigen::Matrix<double,4,1> stauts_update(); }; Eigen::Matrix<double,4,1> KalmanFilter::X_last= Eigen::Matrix<double,4, 1>::Zero(); Eigen::Matrix<double,4,4> KalmanFilter::P_last= Eigen::Matrix<double,4, 4>::Zero(); #endif kalman.cpp #include "kalman.h" KalmanFilter::KalmanFilter(){ } KalmanFilter::~KalmanFilter(){ } KalmanFilter::KalmanFilter(const double ProcessNoise_Q ,const double MeasureNoise_R): Noise_Q(ProcessNoise_Q), Noise_R(MeasureNoise_R){ this->trans_A <<1, 0, 0, 0, 0 ,1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ; this->meas_H << 1, 0, 0, 0, 0 ,1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ; } Eigen::Matrix<double,4,1> KalmanFilter::updata_paramount(Eigen::Matrix<double, 4, 1> Data){ measure_Z = Data; return this->stauts_update(); } Eigen::Matrix<double,4,1> KalmanFilter::stauts_update(){ X_pre = trans_A * X_last; P_pre = trans_A * P_last * trans_A.transpose() + Noise_Q * trans_A; Eigen::Matrix4d temp = ( meas_H * P_pre * meas_H.transpose() + Noise_R * meas_H ); Kg = P_pre * meas_H.transpose() * temp.inverse(); X_now = X_pre + Kg*(measure_Z - meas_H * X_pre ); P_update = ( trans_A - Kg ) * P_pre; X_last = X_now; P_last = P_update; return X_now; }