一阶、高阶卡尔曼滤波器

整理的KF

#include "Kalman_filter.h"


using namespace std;



/**
* @brief Init_KalmanInfo   初始化滤波器的初始值
* @param info  滤波器指针
* @param Q 预测噪声方差 由系统外部测定给定
* @param R量噪声方差 由系统外部测定给定
*/

void Init_kalman_Info(kalman_Info* info,double Q,double R)
{
    info->A = 1;  //标量卡尔曼
    info->H = 1;  //
    info->P = 10;  //后验状态估计值误差的方差的初始值（不要为0问题不大）
    info->Q = Q;    //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
    info->R = R;    //测量（观测）噪声方差 可以通过实验手段获得
    info->filterValue = 0;// 测量的初始值

}

double KalmanFilter1D(kalman_Info* kalmanInfo, double lastMeasurement)
{

    //cout<<"don't forget input Q and R"<<endl;
    //预测下一时刻的值
    double predictValue = kalmanInfo->A* kalmanInfo->filterValue;
    //x的先验估计由上一个时间点的后验估计值和输入信息给出，此处需要根据基站高度做一个修改

    //求协方差
    kalmanInfo->P = kalmanInfo->A*kalmanInfo->A*kalmanInfo->P + kalmanInfo->Q;
    //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q
    double preValue = kalmanInfo->filterValue;  //记录上次实际坐标的值

    //计算kalman增益
    kalmanInfo->kalmanGain = kalmanInfo->P*kalmanInfo->H / (kalmanInfo->P*kalmanInfo->H*kalmanInfo->H + kalmanInfo->R);
    //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    //修正结果，即计算滤波值
    kalmanInfo->filterValue = predictValue + (lastMeasurement - predictValue)*kalmanInfo->kalmanGain;
    //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
    //更新后验估计
    kalmanInfo->P = (1 - kalmanInfo->kalmanGain*kalmanInfo->H)*kalmanInfo->P;
    //计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]

    return  kalmanInfo->filterValue;
}


/**
* @brief EKF   EKF
* @param info  滤波器指针
* @param Q 预测噪声方差 由系统外部测定给定
* @param R量噪声方差  由系统外部测定给定
*/
KalmanFilter::KalmanFilter(int stateSize_ = 0, int measSize_ = 0, int uSize_=0) :stateSize(stateSize_), measSize(measSize_), uSize(uSize_)
{
    if (stateSize == 0 || measSize == 0)
    {
        std::cerr << "Error, State size and measurement size must bigger than 0\n";
    }

    x.resize(stateSize);
    x.setZero();

    A.resize(stateSize, stateSize);
    A.setIdentity();

    u.resize(uSize);
    u.transpose();
    u.setZero();

    B.resize(stateSize, uSize);
    B.setZero();

    P.resize(stateSize, stateSize);
    P.setIdentity();

    H.resize(measSize, stateSize);
    H.setZero();

    z.resize(measSize);
    z.setZero();

    Q.resize(stateSize, stateSize);
    Q.setZero();

    R.resize(measSize, measSize);
    R.setZero();
}

void KalmanFilter::init(Eigen::VectorXd &x_, Eigen::MatrixXd& P_, Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_)
{
    x = x_;
    P = P_;
    R = R_;
    Q = Q_;
}
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_, Eigen::MatrixXd &B_, Eigen::VectorXd &u_)
{
    A = A_;
    B = B_;
    u = u_;
    x = A*x + B*u;
    Eigen::MatrixXd A_T = A.transpose();
    P = A*P*A_T + Q;
    return x;
}

/***********
 * 
 * @param A_    state Matrix
 * @return      predicted x
 */
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_)
{
    A = A_;
    x = A*x;
    Eigen::MatrixXd A_T = A.transpose();
    P = A*P*A_T + Q;
//  cout << "P-=" << P<< endl;
    return x;
}
/***************
 *
 * @param H_        State transition matrix
 * @param z_meas    Measured data
 */

void KalmanFilter::update(Eigen::MatrixXd& H_,Eigen::VectorXd z_meas)
{
    H = H_;
    Eigen::MatrixXd temp1, temp2,Ht;
    Ht = H.transpose();
    temp1 = H*P*Ht + R;
    temp2 = temp1.inverse();//(H*P*H'+R)^(-1)
    Eigen::MatrixXd K = P*Ht*temp2;
    z = H*x;
    x = x + K*(z_meas-z);
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize, stateSize);
    P = (I - K*H)*P;
    cout << "P=" << P << endl;
}
/**************************
 *
 * @param x_          Input State variables
 * @param z_measure   Measured variables
 * @param A_          The state matrix
 * @param B_          gating matrix
 * @param u_          Control variables
 * @param P_          Covariance matrix
 * @param H_          State transition matrix
 * @param R_          测量噪声协方差  滤波器的已知条件
 * @param Q_          过程激励噪声协方差   状态转移协方差矩阵
 * @return x          Filtering results
 */
Eigen::VectorXd KalmanFilter::KalmanXDFilter(
        Eigen::VectorXd x_,
        Eigen::VectorXd z_measure,
        Eigen::MatrixXd A_,
        Eigen::MatrixXd B_,
        Eigen::VectorXd u_,
        Eigen::MatrixXd P_,//coveriance
        Eigen::MatrixXd H_,
        Eigen::MatrixXd R_,//measurement noise covariance
        Eigen::MatrixXd Q_//process noise covariance
        )
{
    x = x_;
    P = P_;
    R = R_;
    Q = Q_;
    A = A_;
    B = B_;
    u = u_;
    H = H_;
    //1111   predicted
    x=A*x+B*u;
    z = H*x;
    Eigen::MatrixXd A_T = A.transpose();
    //22222222222
    P = A*P*A_T  + Q ;
    ///3333333
    Eigen::MatrixXd temp1, temp2,Ht;
    Ht = H.transpose();
    temp1 = H*P*Ht+R;
    temp2 = temp1.inverse();
    Eigen::MatrixXd K;
    K = P*Ht*temp2;
    //44444444444
    x  = x  +  K*(z_measure - z);
    //5555555
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize,stateSize);
    P  = (I-K*H)*P;
    return x;

}

//
// Created by wdb1 on 2021/6/17.
//

#ifndef MARSCAR_KALMAN_FILTER_H
#define MARSCAR_KALMAN_FILTER_H

#include "ros/ros.h"
#include "filters/filter_base.h"
#include <stdio.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

typedef struct {

    double filterValue;  //k-1时刻的滤波值，即是k-1时刻的值
    double kalmanGain; //   Kalamn增益
    double A;  // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)
    double H;  // z(n)=H*x(n)+w(n),w(n)~N(0,R)
    double Q;   //预测过程噪声偏差的方差
    double R;   //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
    double P;    //估计误差协方差

} kalman_Info;

class KalmanFilter
{
private:
    int stateSize; //state variable's dimenssion
    int measSize; //measurement variable's dimession
    int uSize; //control variables's dimenssion
    Eigen::VectorXd x;
    Eigen::VectorXd z;
    Eigen::MatrixXd A;
    Eigen::MatrixXd B;
    Eigen::VectorXd u;
    Eigen::MatrixXd P;//coveriance
    Eigen::MatrixXd H;
    Eigen::MatrixXd R;//measurement noise covariance
    Eigen::MatrixXd Q;//process noise covariance
public:
    KalmanFilter(int stateSize_, int measSize_,int uSize_);
    ~KalmanFilter(){}
    void init(Eigen::VectorXd &x_, Eigen::MatrixXd& P_,Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_);
    Eigen::VectorXd predict(Eigen::MatrixXd& A_);
    Eigen::VectorXd predict(Eigen::MatrixXd& A_, Eigen::MatrixXd &B_, Eigen::VectorXd &u_);
    void update(Eigen::MatrixXd& H_,Eigen::VectorXd z_meas);
    Eigen::VectorXd KalmanXDFilter(
            Eigen::VectorXd x,
            Eigen::VectorXd z,
            Eigen::MatrixXd A,
            Eigen::MatrixXd B,
            Eigen::VectorXd u,
            Eigen::MatrixXd P,//coveriance
            Eigen::MatrixXd H,
            Eigen::MatrixXd R,//measurement noise covariance
            Eigen::MatrixXd Q//process noise covariance
            );


};


void Init_kalman_Info(kalman_Info* info,double Q,double R);
double KalmanFilter1D(kalman_Info* kalmanInfo, double lastMeasurement);



#endif //MARSCAR_KALMAN_FILTER_H