容积卡尔曼滤波算法 CKF

原创

已于 2022-05-06 21:04:42 修改 · 4.3k 阅读

29 ·

CC 4.0 BY-SA版权

文章标签：

#矩阵 #算法 #c++ #matlab

于 2022-05-06 21:00:17 首次发布

该博客详细介绍了如何在C++中实现容积卡尔曼滤波（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.47元/天解锁文章