卡尔曼滤波C++代码
卡尔曼滤波预测和更新两部分,卡尔曼滤波主要实现预测过程个更新过程;因此在代码实现的时候需要完成四部分
- 变量定义
2)变量形状设置(由构造函数实现)
3) 初始化,送入初始的状态量和协方差
4) 预测过程
使用状态转化方程完成状态量的先验估计,然后根据初始的协方差更新先验估计的协方差;
5)更新过程
首先需要从系统中得到当前时刻的观测量(该值是直接从系统中得到的,不需要再代码计算,因为是使用卡尔曼增益来平衡真实观察量与先验估计值的比例);这里需要观测矩阵,和观测噪声的协方差;
#define _MYKALMAN_H
#define _MYKALMAN_H
#include<iostream>
#include <Eigen\Dense>
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; //先验估计协方差
Eigen::MatrixXd H; //观测矩阵
Eigen::MatrixXd R; //测量噪声协方差
Eigen::MatrixXd Q; //过程噪声协方差
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);
};
KalmanFilter::KalmanFilter(int stateSize_ = 0, int measSize_ = 0, int uSize_ = 0) :stateSize(stateSize_), measSize(measSize_), uSize(uSize_)
// sateSize状态量个数
// 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;//根据时刻t-1的状态由状态转换举着预测时刻t的先验估计值,
//因为当前是使用系统的状态转移矩阵来进行预测时刻t的预测值,并不清楚过程噪声如何,所以不适用过程噪声
Eigen::MatrixXd A_T = A.transpose();
P = A * P * A_T + Q;//时刻t先验估计值的的协方差,Q为过程噪声的协方差
return 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;
}
void KalmanFilter::update(Eigen::MatrixXd& H_, Eigen::VectorXd z_meas)
// H_ 观测矩阵
//z_means 观测量,由系统真实观测输入
{
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);//Identity()单位阵
P = (I - K * H) * P;
// cout << "P=" << P << endl;
}