无迹卡尔曼滤波算法 UKF

无迹卡尔曼滤波的仿真案例，参考书为北航宇航学院王可东老师的Kalman滤波基础及Matlab仿真
一、状态模型：

二、测量模型：

状态方程和测量方程中的噪声均为期望为零的白噪声。
三、状态模型和测量模型的噪声矩阵及初始状态及协方差矩阵：

四、C++ 仿真源码：
UKF.h

#pragma once
#include <fstream>     
#include <string>   
#include <iostream>
#include <Eigen/Dense>
#include "RandomGenerate.h"

class UKF
{
public:
	UKF();
	virtual ~UKF();
	void Filter();                // 无迹卡尔曼滤波主函数

private:
	void Initialize();                                                                                     // 初始化相关参数
	void GenerateRealx(double h);                                                                          // 生成状态向量真实值
	void GenerateRealz();																				   // 生成测量向量值

	Eigen::MatrixXd GenerateSigmaPoint(const Eigen::Matrix2d& PestChol);                                   // 生成 Sigme 点
	Eigen::MatrixXd Nonlinearf(double h, const Eigen::MatrixXd& SigmaPointest);                            // 非线性状态方程(非线性映射)
	void Prediction(const Eigen::MatrixXd& SigmaPointpre);                                                 // 一步预测
	Eigen::MatrixXd Nonlinearh(const Eigen::MatrixXd& SigmaPointpre);                                      // 非线性测量方程
	void Update(const Eigen::MatrixXd& SigmaPointpre, const Eigen::MatrixXd& SigmaPointz);                 // 测量更新

private:
	int n;

	double alpha;

	double beta;

	double kappa;

	double lambda;

	double gama;

	Eigen::VectorXd Wm;

	Eigen::VectorXd Wc;

	Eigen::Vector2d xpre;          // 状态向量预测值

	Eigen::Matrix2d Ppre;          // 状态协方差矩阵预测值

	Eigen::Matrix2d Q;             // 状态方程噪声矩阵

	Eigen::Matrix2d R;             // 测量方程噪声矩阵

	Eigen::Matrix2d K;             // 卡尔曼增益矩阵

	Eigen::Vector2d xest;          // 状态向量估计值

	Eigen::Matrix2d Pest;          // 状态协方差矩阵估计值

	Eigen::Vector2d xreal;

	Eigen::Vector2d zreal;

private:
	std::string FileName;          // 文件名

	std::ofstream outFile;         // 文件路径
};

UKF.cpp

#include "UKF.h"

UKF::UKF() : FileName("./FilterUKF.txt"), outFile(FileName, std::ios::out)
{
	// 初始化相关参数
	Initialize();
}
UKF::~UKF()
{

}

// 初始化相关参数
void UKF::Initialize()
{
	// 初始化状态方程噪声矩阵
	Q << 0.01, 0,
		 0, 0.0001;

	// 初始化测量方程噪声矩阵
	R << 0.1, 0,
		 0, 0.1;

	// 初始化状态协方差矩阵估计值
	Pest << 1, 0,
		    0, 1;

	// 初始化状态向量估计值
	xest << 1, 0;

	// 真实值
	xreal = xest;

	// others
	n = 2;
	alpha = 0.1;
	beta = 2;
	kappa = 1;
	lambda = alpha * alpha * (n + kappa) - n;
	gama = sqrt(n + lambda);
	
	// weight
	Eigen::VectorXd W0(5);
	W0 << 0, 0, 0, 0, 0;	
	Wm = W0;
	Wc = W0;
	Wm(0) = lambda / (n + lambda);
	Wc(0) = lambda / (n + lambda) + 1 - alpha * alpha + beta;
	for (int i = 1; i != 5; i++)
	{
		Wm(i) = 1.0 / (2*(n + lambda));
		Wc(i) = 1.0 / (2*(n + lambda));
	}
	
	return;
}

// 生成状态向量真实值
void UKF::GenerateRealx(double h)
{
	xreal(0) = xreal(0) + h * xreal(1) + sqrt(Q(0, 0)) * getRandom();
	xreal(1) = -10 * h * sin(xreal(0)) + (1 - h) * xreal(1) + sqrt(Q(1, 1)) * getRandom();
	return;
}

// 生成测量向量值
void UKF::GenerateRealz()
{
	zreal(0) = 2 * sin(xreal(0)/2.0) + sqrt(R(0, 0)) * getRandom();;
	zreal(1) = 0.5 * xreal(0) + sqrt(R(1, 1)) * getRandom();
	return;
}

// 生成 Sigma 点
Eigen::MatrixXd UKF::GenerateSigmaPoint(const Eigen::Matrix2d& PestChol)
{
	Eigen::MatrixXd SigmaPointest(2, 5);
	SigmaPointest.col(0) = xest;
	SigmaPointest.col(1) = xest + gama * PestChol.col(0);
	SigmaPointest.col(2) = xest + gama * PestChol.col(1);
	SigmaPointest.col(3) = xest - gama * PestChol.col(0);
	SigmaPointest.col(4) = xest - gama * PestChol.col(1);

	return SigmaPointest;
}

// 非线性状态方程(非线性映射)
Eigen::MatrixXd UKF::Nonlinearf(double h, const Eigen::MatrixXd &SigmaPointest)
{
	Eigen::MatrixXd SigmaPointpre(2, 5);
	for (int i = 0; i != 5; i++)
	{
		SigmaPointpre(0,i) = SigmaPointest(0,i) + h * SigmaPointest(1,i);
		SigmaPointpre(1,i) = -10 * h * sin(SigmaPointest(0,i)) + (1 - h) * SigmaPointest(1,i);
	}

	return SigmaPointpre;
}

// 一步预测
void UKF::Prediction(const Eigen::MatrixXd& SigmaPointpre)
{	
	xpre = SigmaPointpre * Wm;
	Eigen::Matrix2d Pxx;
	Pxx << 0, 0, 0, 0;
	for (int i = 0; i != 5; i++)
	{
		Eigen::Vector2d temp = SigmaPointpre.col(i) - xpre;
		Eigen::MatrixXd tempmat(2,1);
		for (int j = 0; j != temp.size(); j++)
		{
			tempmat(j, 0) = temp(j);
		}
		Eigen::Matrix2d mat = tempmat * tempmat.transpose();
		Pxx += Wc(i) * mat;
	}

	Ppre = Pxx + Q;
	return;
}

// 非线性测量方程
Eigen::MatrixXd UKF::Nonlinearh(const Eigen::MatrixXd& SigmaPointpre)
{
	Eigen::MatrixXd SigmaPointz(2, 5);
	for (int i = 0; i != 5; i++)
	{
		SigmaPointz(0,i) = 2 * sin(0.5 * SigmaPointpre(0,i));
		SigmaPointz(1,i) = 0.5 * SigmaPointpre(0,i);
	}

	return SigmaPointz;
}

// 测量更新
void UKF::Update(const Eigen::MatrixXd& SigmaPointpre, const Eigen::MatrixXd& SigmaPointz)
{
	Eigen::Vector2d zpre = SigmaPointz * Wm;
	Eigen::Matrix2d Pzz;
	Pzz << 0, 0, 0, 0;
	for (int i = 0; i != 5; i++)
	{
		Eigen::Vector2d temp = SigmaPointz.col(i) - zpre;
		Eigen::MatrixXd tempmat(2, 1);
		for (int j = 0; j != temp.size(); j++)
		{
			tempmat(j, 0) = temp(j);
		}
		Eigen::Matrix2d mat = tempmat * tempmat.transpose();
		Pzz += Wc(i) * mat;
	}

	Eigen::Matrix2d Pvv = Pzz + R;
	Eigen::Matrix2d Pxz;
	Pxz << 0, 0, 0, 0;
	for (int i = 0; i != 5; i++)
	{
		Eigen::Vector2d temp1 = SigmaPointpre.col(i) - xpre;
		Eigen::Vector2d temp2 = SigmaPointz.col(i) - zpre;
		Eigen::MatrixXd temp1mat(2, 1);
		Eigen::MatrixXd temp2mat(2, 1);
		for (int j = 0; j != temp1.size(); j++)
		{
			temp1mat(j, 0) = temp1(j);
			temp2mat(j, 0) = temp2(j);
		}
		Eigen::Matrix2d mat = temp1mat * temp2mat.transpose();
		Pxz += Wc(i) * mat;
	}

	K = Pxz * Pvv.inverse();
	xest = xpre + (K * (zreal - zpre));
	Pest = Ppre - (K * Pvv * K.transpose());
	return;
}

// 无迹卡尔曼滤波主函数
void UKF::Filter()
{
	std::cout << "请输入滤波时间：" << std::endl;
	double time;
	std::cin >> time;
	double h = 0.05;
	int num = int(time / h);

	for (int i = 0; i != num; i++)
	{
		// 生成状态向量真实值
		GenerateRealx(h);

		// 生成测量向量值
		GenerateRealz();

		// 矩阵下三角分解
		Eigen::Matrix2d PestChol = Pest.llt().matrixL();

		// 生成 Sigma 点
		Eigen::MatrixXd SigmaPointest = GenerateSigmaPoint(PestChol);

		// Sigma 点经过非线性状态方程(非线性映射)
		Eigen::MatrixXd SigmaPointpre = Nonlinearf(h, SigmaPointest);

		// 一步预测
		Prediction(SigmaPointpre);

		//  一步预测过后的 Sigma 点经过非线性测量方程
		Eigen::MatrixXd SigmaPointz = Nonlinearh(SigmaPointpre);

		// 测量更新
		Update(SigmaPointpre, SigmaPointz);

		// 保存到文件
		outFile << xreal(0) << ", " << xreal(1) << ", "<<xest(0) << ", " << xest(1) << ", " << xpre(0) << ", " << xpre(1) << std::endl;

		// 输出到控制台
		//std::cout<< i <<":  " << abs(xreal(0)-xest(0)) << ", " << abs(xreal(1) - xest(1)) << std::endl;
	}
	return;
}

demo.cpp

#include "UKF.h"
#include "EKF.h"

int main()
{
	UKF ukf;
	ukf.Filter();

	//EKF ekf;
	//ekf.Filter();

	system("pause");
	return 0;
}

五、仿真结果：

%% 测试 C++ 程序的可行性。
clear;
clc;

%% 读入C++数据
x = dlmread('Filter.txt');
n = length(x(:,1));
t = 1 : n;

%% 状态
figure;
plot(t, x(:,1), '*-');
hold on;
plot(t, x(:,2), 'o-');
legend('real1','real2');
title('状态');
xlabel('时间/s');
grid on;

%% 状态1
figure;
plot(t, x(:,1), 's-', t, x(:,3), 'o-', t, x(:,5),'*-');
legend('real1','est1','pre1');
title('状态1');
xlabel('时间/s');
grid on;

%% 状态2
figure;
plot(t, x(:,2), 's-', t, x(:,4), 'o-', t, x(:,6),'*-');
legend('real2','est2','pre2');
title('状态2');
xlabel('时间/s');
grid on;

%% 状态1误差
figure;
plot(t, x(:,1)-x(:,3), 'o-', t, x(:,1)-x(:,5),'*-');
legend('est1','pre1');
title('状态1误差');
xlabel('时间/s');
grid on;

%% 状态2误差
figure;
plot(t, x(:,2)-x(:,4), 'o-', t, x(:,2)-x(:,6),'*-');
legend('est2','pre2');
title('状态2误差');
xlabel('时间/s');
grid on;