一阶、高阶卡尔曼滤波器
程序员文章站
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
上一篇: DP之完全背包问题
下一篇: 卡尔曼滤波算法及C语言实现_源代码