无迹卡尔曼滤波的仿真案例，参考书为北航宇航学院王可东老师的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;// othersn = 2;alpha = 0.1;beta = 2;kappa = 1;lambda = alpha * alpha * (n + kappa) - n;gama = sqrt(n + lambda);// weightEigen::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;%% 状态1figure;plot(t, x(:,1), 's-', t, x(:,3), 'o-', t, x(:,5),'*-');legend('real1','est1','pre1');title('状态1');xlabel('时间/s');grid on;%% 状态2figure;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;
如果觉得《无迹卡尔曼滤波算法 UKF》对你有帮助，请点赞、收藏，并留下你的观点哦！
