C++ 扩展卡尔曼滤波

ExtendedKalmanFilter类

/*扩展卡尔曼滤波器*/
#pragma once
#include <Eigen/Dense>
#include <cmath>

using namespace std;
using namespace Eigen;

class ExtendedKalmanFilter 
{
public:
	//构造函数
	ExtendedKalmanFilter() {};
                                                         
	//析构函数
	~ExtendedKalmanFilter() {}  

	//状态初始化
	void x_init(VectorXd x_in)
	{
		x = x_in;
	}

	//状态协方差矩阵初始化
	MatrixXd P_init(MatrixXd P_in)
	{
		P = P_in;
		return P;
	}
	//状态转移矩阵赋值
	void SetF(MatrixXd F_in) 
	{
		F = F_in;
	}

	//控制矩阵赋值
	void SetB(MatrixXd B_in)
	{
		B = B_in;
	}

	//过程噪声矩阵赋值
	void SetQ(MatrixXd Q_in)
	{
		Q = Q_in;
	}

	//量测噪声矩阵赋值
	void SetR(MatrixXd R_in)
	{
		R = R_in;
	}

	//预测更新
	VectorXd Prediction()
	{
		//预测更新状态
		x = F * x + B * u;
		//预测更新状态协方差
		P = F * P * F.transpose() + Q;
		return x;
	}

	//读入测量数据
	VectorXd Read_measurement(VectorXd z_in)
	{
		z = z_in;
		z_to << z(1)*cos(z(2)), z(1)*sin(z(2));
		return z_to;
	}

	//计算雅可比矩阵
	void CalculateJacobianMatrix()
	{
		double px = z(1)*cos(z(2));
		double py = z(1)*sin(z(2));
		double vx = z(3)*cos(z(2));
		double vy = z(3)*sin(z(2));
		//获取预测更新状态量
		double c1 = pow(px,2) + pow(py, 2);
		double c2 = sqrt(c1);
		double c3 = c1 * c2;
		MatrixXd Hj(3, 4);
		//避免除0的情况
		if (c1 == 0 || c2 == 0 || c3 == 0)
		{
			Hj << 0, 0, 0, 0;
				  0, 0, 0, 0,
				  0, 0, 0, 0;
		}
		else
		{
			Hj << (px / c2), (py / c2), 0, 0,
				-(py / c1), (px / c1), 0, 0,
				py * (py*vx - px*vy) / c3,px*(px*vy - vx*py) / c3,py / c2;
		}
	}

	//测量更新
	VectorXd  MeasurementUpdate()
	{
		VectorXd y = z - Hj * x;
		//更新卡尔曼增益
		MatrixXd S = Hj * P * Hj.transpose() + R;
		MatrixXd K = P * Hj.transpose() * S.inverse();
		//测量更新状态
		x = x + (K * y);
		//更新协方差阵
		P = (MatrixXd::Identity(4, 4) - K * Hj) * P;
		return x;
	}

private:
	//状态向量
	VectorXd x;

	//状态协方差矩阵
	MatrixXd P;     

	//测量向量
	VectorXd z;     

	//测量向量转换到4维
	VectorXd z_to;

	//状态转移矩阵
	MatrixXd F;     

	//控制矩阵
	MatrixXd B;

	//过程噪声矩阵
	MatrixXd Q;

	//雅可比矩阵
	MatrixXd Hj;

	//量测噪声协方差矩阵
	MatrixXd R;

	//外部控制
	int u = 1;
};

main.cpp

/*扩展卡尔曼滤波器的调用*/
#include "extended_kalman_filter.h"
#include <iostream>           //文件导入
#include <fstream>            //文件导出
#include <Eigen/Dense>        //矩阵运算
#include <vector>             //动态数组
#include <cmath>              //数学公式
#include <ctime>              //程序计时
#include <regex>              //正则表达式
#include <string>             //导入数据用

using namespace std;
using namespace Eigen;

int main()
{
	// ******************************开始计时************************************
	clock_t start, stop;
	double duration;
	start = clock();
	double delta_t = 0.1;
	ofstream fout("position.txt");    //""中是txt文件路径

	// ******************************导入数据************************************/
	//txt读入数据到vector中
	vector<float> temp_line;
	vector<vector<float>> Vec_Dti;
	string line;
	ifstream in("data.txt");     //读入文件
	//正则式表达，浮点数								
	regex pat_regex("\\d+(\\.\\d+)?");
	while (getline(in, line))
	{   //按行读取
		for (sregex_iterator it(line.begin(), line.end(), pat_regex), end_it; it != end_it; ++it)
		{
			//表达式匹配，匹配一行中所有满足条件的字符
			temp_line.push_back(stof(it->str()));  //将数据转化为float型并存入一维vector中
		}
		Vec_Dti.push_back(temp_line);              //保存所有数据
		temp_line.clear();
	}
	//将所有数据保存到data矩阵中
	MatrixXf data(200, 8);
	for (int i = 0; i < 200; i++)
		data.row(i) = VectorXf::Map(&Vec_Dti[i][0], Vec_Dti[i].size());
	//cout << data;
	putchar('\n');
	putchar('\n');
	putchar('\n');
	//cout << data.middleRows(0,3);  //显示第1到3行 [0,3) 不显示第四行

	//定义类对象 ekf
	ExtendedKalmanFilter EKF;

	// ******************************初始化************************************
	//初始化状态
	VectorXd x_in;
	x_in << data(0, 1), data(0, 2), data(0, 3), 0;
	EKF.x_init(x_in);
	//初始化状态协方差矩阵
	MatrixXd P_in;
	P_in << 1, 0, 0, 0,
			0, 1, 0, 0,
			0, 0, 1000, 0,
			0, 0, 0, 1000;
	MatrixXd A = EKF.P_init(P_in);
	cout << A;              
	//状态转移矩阵赋值
	MatrixXd F_in;
	F_in << 1, 0, delta_t, 0,
			0, 1, 0, delta_t,
			0, 0, 1, 0,
			0, 0, 0, 1;
	EKF.SetF(F_in);
	//控制矩阵赋值
	MatrixXd B_in;
	B_in << pow(delta_t, 2) / 2, pow(delta_t, 2) / 2,
			delta_t, delta_t;
	EKF.SetB(B_in);
	//过程噪声矩阵赋值
	MatrixXd Q_in;
	Q_in << pow(delta_t, 2) / 4, 0, pow(delta_t, 3) / 2, 0,
			0, pow(delta_t, 2) / 4, 0, pow(delta_t, 3) / 2,
			pow(delta_t, 3) / 2, 0, pow(delta_t, 2), 0,
			0, pow(delta_t, 3) / 2, 0, pow(delta_t, 2),
	EKF.SetQ(Q_in);
	//量测噪声矩阵赋值
	MatrixXd R_in;
	R_in << 0.09, 0, 0,
			0, 0.005, 0,
			0, 0, 0.09;
	EKF.SetR(R_in);

	// ******************************开始迭代************************************
	cout << "含噪声测量" << "  " << "状态预测" << "  " << "最优估计" << "  " << "真实值" << endl;
	for (int i = 0; i < 200; i++)
	{
		if (data(i, 0) == 2)
		{
			//预测更新状态、状态协方差阵
			VectorXd x_p = EKF.Prediction();
			//Radar测量数据
			VectorXd z_in;
			z_in << data(i, 1), data(i, 2), data(i, 3);
			VectorXd z_t = EKF.Read_measurement(z_in);
			//计算雅可比矩阵
			EKF.CalculateJacobianMatrix();
			//测量更新
			VectorXd x_e = EKF.MeasurementUpdate();
			// ******************************显示结果************************************
			cout << z_t(0) << "  " << x_p(0) << "  " << x_e(0) << "  " << data(i, 4) << endl;
			fout << z_t(0) << "  " << x_p(0) << "  " << x_e(0) << "  " << data(i, 4) << endl;
		}
	}
	fout.close();
	
	// ******************************结束计时************************************
	putchar('\n');
	putchar('\n');
	putchar('\n');

	stop = clock();                     
	duration = ((double)(stop - start)) / CLK_TCK; //CLK_TCK = 1000, 6x86_64-w64-mingw32-gcc.exe编译器
	cout << "程序用时" << duration << endl;

	return 0;
}