欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

一阶、高阶卡尔曼滤波器

程序员文章站 2022-07-12 09:34:06
...

整理的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

相关标签: C++ 卡尔曼滤波