容积卡尔曼滤波算法 CKF

容积卡尔曼仿真案例
一、状态模型：

二、测量模型：

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

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

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

class CKF
{
public:
	CKF();
	virtual ~CKF();
	void Filter();                // 容积卡尔曼滤波主函数

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

	Eigen::MatrixXd GenerateCubaPoint(const Eigen::Matrix2d& PestChol);                                    // 生成容积点
	Eigen::MatrixXd Nonlinearf(double h, const Eigen::MatrixXd& CubaPointest);                             // 非线性状态方程(非线性映射)
	void Prediction(const Eigen::MatrixXd& CubaPointpre);                                                  // 一步预测

	Eigen::MatrixXd GenerateCubaPoint2(const Eigen::Matrix2d& PpreChol);                                   // 生成容积点
	Eigen::MatrixXd Nonlinearh(const Eigen::MatrixXd& CubaPointz_pre);                                     // 非线性测量方程
	void Update(const Eigen::MatrixXd& CubaPointz_pre, const Eigen::MatrixXd& CubaPointz);                 // 测量更新

private:
	int n;

	double gama;

	Eigen::VectorXd Wm;

	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;         // 文件路径
};

CKF.cpp

#include "CKF.h"

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

}

// 初始化相关参数
void CKF::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;
	gama = sqrt(n / 2.0);

	// weight
	Eigen::VectorXd W0(4);
	W0 << 0, 0, 0, 0;
	Wm = W0;
	for (int i = 0; i != 4; i++)
	{
		Wm(i) = 1.0 / (2 * n);
	}

	return;
}

// 生成状态向量真实值
void CKF::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 CKF::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;
}

// 生成容积点
Eigen::MatrixXd CKF::GenerateCubaPoint(const Eigen::Matrix2d& PestChol)
{
	Eigen::MatrixXd CubaPointest(2, 4);
	CubaPointest.col(0) = xest + gama * PestChol.col(0);
	CubaPointest.col(1) = xest + gama * PestChol.col(1);
	CubaPointest.col(2) = xest - gama * PestChol.col(0);
	CubaPointest.col(3) = xest - gama * PestChol.col(1);

	return CubaPointest;
}

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

	return CubaPointpre;
}

// 一步预测
void CKF::Prediction(const Eigen::MatrixXd& CubaPointpre)
{
	xpre = CubaPointpre * Wm;
	Eigen::Matrix2d Pxx;
	Pxx << 0, 0, 0, 0;
	for (int i = 0; i != 4; i++)
	{
		Eigen::Vector2d temp = CubaPointpre.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 += Wm(i) * mat;
	}

	Ppre = Pxx + Q;
	return;
}

// 生成容积点
Eigen::MatrixXd CKF::GenerateCubaPoint2(const Eigen::Matrix2d& PpreChol)
{
	Eigen::MatrixXd CubaPointz_pre(2, 4);
	CubaPointz_pre.col(0) = xpre + gama * PpreChol.col(0);
	CubaPointz_pre.col(1) = xpre + gama * PpreChol.col(1);
	CubaPointz_pre.col(2) = xpre - gama * PpreChol.col(0);
	CubaPointz_pre.col(3) = xpre - gama * PpreChol.col(1);

	return CubaPointz_pre;
}

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

	return CubaPointz;
}

// 测量更新
void CKF::Update(const Eigen::MatrixXd& CubaPointz_pre, const Eigen::MatrixXd& CubaPointz)
{
	Eigen::Vector2d zpre = CubaPointz * Wm;
	Eigen::Matrix2d Pzz;
	Pzz << 0, 0, 0, 0;
	for (int i = 0; i != 4; i++)
	{
		Eigen::Vector2d temp = CubaPointz.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 += Wm(i) * mat;
	}

	Eigen::Matrix2d Pvv = Pzz + R;
	Eigen::Matrix2d Pxz;
	Pxz << 0, 0, 0, 0;
	for (int i = 0; i != 4; i++)
	{
		Eigen::Vector2d temp1 = CubaPointz_pre.col(i) - xpre;
		Eigen::Vector2d temp2 = CubaPointz.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 += Wm(i) * mat;
	}

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

// 容积卡尔曼滤波主函数
void CKF::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();

		// 生成容积点
		Eigen::MatrixXd CubaPointest = GenerateCubaPoint(PestChol);

		// 容积点点经过非线性状态方程(非线性映射)
		Eigen::MatrixXd CubaPointpre = Nonlinearf(h, CubaPointest);

		// 一步预测
		Prediction(CubaPointpre);

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

		// 生成容积点
		Eigen::MatrixXd CubaPointz_pre = GenerateCubaPoint2(PpreChol);

		// 容积点经过非线性测量方程
		Eigen::MatrixXd CubaPointz = Nonlinearh(CubaPointz_pre);

		// 测量更新
		Update(CubaPointz_pre, CubaPointz);

		// 保存到文件
		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 "EKF.h"
#include "UKF.h"
#include "CKF.h"

int main()
{
	EKF ekf;
	//ekf.Filter();

	UKF ukf;
	//ukf.Filter();

	CKF ckf;
	ckf.Filter();

	system("pause");
	return 0;
}

五、仿真结果：

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

%% 读入C++数据
x = dlmread('FilterCKF.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;