卡尔曼滤波器实现代码

Posted 2021-01-02 gaoxianzhi

#ifndef KALMANOFLIDAR_H
#define KALMANOFLIDAR_H

#include <EigenDense>
#include <EigenCore>
#include <vector>
#include <iostream>

using namespace std;
using namespace Eigen;

class kalmanFilter
{

public:
    vector<VectorXd> measurements;

    kalmanFilter();
    ~kalmanFilter();

    VectorXd measusre_data(VectorXd single_meas);
    void test();

private:

    VectorXd Measurement_update_0(vector<VectorXd> measurements, int i=0)
    {
        VectorXd z = measurements[i];
        return z;
    }

    VectorXd Cal_measurement_perdcition_error_1_1(VectorXd &x_predict_i, VectorXd &z, MatrixXd &H)//H is the relation of input and output,output can be meausured, input need prediction
    {
         VectorXd y = z - H * x_predict_i;
         return y;
    }

    VectorXd Cal_kalmen_gain_1_2(MatrixXd &P_predict_i, MatrixXd &H, MatrixXd &Rnoise)
    {
        VectorXd S = H * P_predict_i*H.transpose() + Rnoise;
        VectorXd k = P_predict_i * H.transpose()*S.inverse();
        return k;
    }

    VectorXd Correctpredcit_x_2_1(VectorXd &y, VectorXd &x_predict_i, VectorXd &k)
    {
        VectorXd x_predict_calibration = x_predict_i + k * y;
        return x_predict_calibration;
    }

    MatrixXd Correctpredcit_P_2_2(MatrixXd &P_predict_i, VectorXd &k, MatrixXd &I,MatrixXd &H)
    {
        MatrixXd P_predict_calibration_i = (I - k * H)*P_predict_i;
        return P_predict_calibration_i;
    }

    VectorXd Generate_NewPredic_xVector_3_1(VectorXd &x_predict_calibration, VectorXd &unosie, MatrixXd &F)//F is the relation of this time and next time
    {
        VectorXd x_predict_iplus1 = F * x_predict_calibration + unosie;
        return x_predict_iplus1;
    }

    MatrixXd Generate_NewPredic_PMatrix_3_2(MatrixXd &P_predict_calibration_i, MatrixXd &F, MatrixXd &Qnoise)//F is the relation of this time and next time
    {
        MatrixXd P_predict_iplus1= F * P_predict_calibration_i*F.transpose() + Qnoise;
        return P_predict_iplus1;
    }

    void filter(VectorXd &x, MatrixXd &P, VectorXd &unosie, MatrixXd &F, MatrixXd &H, MatrixXd &Rnoise, MatrixXd &I, MatrixXd &Qnoise, int N);

};

 

-------------------------------------------------------------------------------------------------------------------------------------------------------------------

 

 

#include "kalmanoflidar.h"

using namespace std;
using namespace Eigen;

kalmanFilter::kalmanFilter()
{
}

kalmanFilter::~kalmanFilter()
{
}

VectorXd kalmanFilter::measusre_data(VectorXd single_meas)
{
    measurements.push_back(single_meas);
    return single_meas;
}


void kalmanFilter::filter(VectorXd &x, MatrixXd &P, VectorXd &unosie, MatrixXd &F, MatrixXd &H, MatrixXd &Rnoise, MatrixXd &I, MatrixXd &Qnoise,int N)
{
    VectorXd x_predict_i = x;
    MatrixXd P_predict_i = P;
    for (int i = 0;i < N;i++)
    {
        VectorXd z = Measurement_update_0(measurements, i);

        VectorXd y = Cal_measurement_perdcition_error_1_1(x_predict_i, z, H);//H is the relation of input and output

        VectorXd k = Cal_kalmen_gain_1_2(P_predict_i, H, Rnoise);

        VectorXd x_predict_calibration_i = Correctpredcit_x_2_1(y, x_predict_i, k);//F is the relation of this time and next time

        MatrixXd P_predict_calibration_i = Correctpredcit_P_2_2(P_predict_i, k, I,H);

        VectorXd x_predict_iplus1 = Generate_NewPredic_xVector_3_1(x_predict_calibration_i, unosie, F);//F is the relation of this time and next time

        MatrixXd P_predict_iplus1 = Generate_NewPredic_PMatrix_3_2(P_predict_calibration_i, F, Qnoise);//F is the relation of this time and next time

        x_predict_i = x_predict_iplus1;

        P_predict_i = P_predict_iplus1;
    }
}


void kalmanFilter::test()
{
    
VectorXd x;
MatrixXd P;
VectorXd u;
MatrixXd F;
MatrixXd H;
MatrixXd R;
MatrixXd I;
MatrixXd Q;

    printf("hello world ");
    VectorXd my_vector(2);
    my_vector << 10, 20;
    MatrixXd my_matrix(2, 2);
    my_matrix << 1, 2, 3, 4;
    cout << my_matrix << endl;
    cout << my_vector << endl;

    x = VectorXd(2);
    x << 0, 0;

    P = MatrixXd(2, 2);
    P << 1000, 1000, -1000, 1000;

    u = VectorXd(2);
    u << 0, 0;

    F = MatrixXd(2, 2);
    F << 1, 1, 0, 1;

    H = MatrixXd(1, 2);
    H << 1, 0;

    R = MatrixXd(1, 1);
    R << 1;

    I = MatrixXd::Identity(2, 2);

    Q = MatrixXd(2, 2);
    Q << 0, 0, 0, 0;

    //create a list of measurements
    VectorXd single_meas(1);
    single_meas << 1;
    measurements.push_back(single_meas);
    single_meas << 5;
    measurements.push_back(single_meas);
    single_meas << 9;
    measurements.push_back(single_meas);
    single_meas << 13;
    measurements.push_back(single_meas);
    single_meas << 17;
    measurements.push_back(single_meas);
    single_meas << 21;
    measurements.push_back(single_meas);
    single_meas << 25;
    measurements.push_back(single_meas);
    single_meas << 29;
    measurements.push_back(single_meas);
    filter(x, P,u,F,H,R,I, Q,8);
    ;
}



#endif // KALMANOFLIDAR_H