扩展卡尔曼滤波算法 EKF

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

二、测量模型：

状态方程和测量方程中的噪声均为期望为零的白噪声。
三、状态模型和测量模型的线性化(Jacobian矩阵)：

四、状态模型和测量模型的噪声矩阵及初始状态及协方差矩阵：

五、C++ 仿真源码：
EKF.h

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

class EKF
{
public:
	EKF();
	virtual ~EKF();
	void Filter();      // 扩展卡尔曼滤波主函数

private:
	void Initialize();             // 初始化相关参数
	void GenerateXreal(int k);     // 生成状态向量真实值
	void GenerateZreal();          // 生成测量向量值
	
	Eigen::Vector2d Nonlinearf(int k);                                                          // 非线性状态方程(非线性映射)
	Eigen::Matrix2d JacobianMatrixF();                                                          // 计算状态方程雅可比矩阵(线性化矩阵)
	void Prediction(const Eigen::Vector2d& f, const Eigen::Matrix2d& F);                        // 一步预测
	Eigen::Vector2d Nonlinearh();                                                               // 非线性测量方程
	Eigen::Matrix2d JacobianMatrixH();                                                          // 计算测量方程雅可比矩阵(线性化矩阵)
	void Update(const Eigen::Vector2d& h, const Eigen::Matrix2d& H);                            // 测量更新

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

EKF.cpp

#include "EKF.h"

// 构造函数
EKF::EKF() : FileName("./FilterEKF.txt"), outFile(FileName, std::ios::out)
{
	// 初始化相关参数
	Initialize();
}

// 析构函数
EKF::~EKF()
{

}

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

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

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

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

	// 真实值
	xreal = xest;

	return;
}

// 生成状态向量真实值
void EKF::GenerateXreal(int k)
{
	xreal(0) = xreal(1) * sin(xreal(0)) + 0.1 * k + sqrt(Q(0,0)) * getRandom();
	xreal(1) = xreal(0) + cos(xreal(1)) * cos(xreal(1)) - 0.1 * k + sqrt(Q(1, 1)) * getRandom();

	return;
}

// 生成测量向量值
void EKF::GenerateZreal()
{
	zreal(0) = xreal.norm() + sqrt(R(0, 0)) * getRandom();;
	zreal(1) = atan(xreal(0) / xreal(1)) + sqrt(R(1, 1)) * getRandom();

	return;
}

// 非线性状态方程(非线性映射)
Eigen::Vector2d EKF::Nonlinearf(int k)
{
	Eigen::Vector2d f;
	f(0) = xest(1) * sin(xest(0)) + 0.1 * k;
	f(1) = xest(0) + cos(xest(1)) * cos(xest(1)) - 0.1 * k;
	return f;
}

// 计算状态方程雅可比矩阵(线性化矩阵)
Eigen::Matrix2d EKF::JacobianMatrixF()
{
	Eigen::Matrix2d F;
	F(0, 0) = xest(1) * cos(xest(0));
	F(0, 1) = sin(xest(0));
	F(1, 0) = 1.0;
	F(1, 1) = -sin(2*xest(1));
	return F;
}

// 一步预测
void EKF::Prediction(const Eigen::Vector2d &f, const Eigen::Matrix2d &F)
{
	// 状态向量预测值
	xpre = f;

	Eigen::Matrix2d Pxx = F * Pest * F.transpose();

	// 状态协方差矩阵预测值
	Ppre = Pxx + Q;

	return;
}

// 非线性测量方程
Eigen::Vector2d EKF::Nonlinearh()
{
	Eigen::Vector2d h;
	h(0) = xpre.norm();
	h(1) = atan(xpre(0) / xpre(1));
	return h;
}

// 计算测量方程雅可比矩阵(线性化矩阵)
Eigen::Matrix2d EKF::JacobianMatrixH()
{
	Eigen::Matrix2d H;
	double a = xpre.norm();
	H(0, 0) = xpre(0) / a;
	H(0, 1) = xpre(1) / a;
	H(1, 0) = xpre(1) / (a * a);
	H(1, 1) = -xpre(0) / (a * a);
	return H;
}

// 测量更新
void EKF::Update(const Eigen::Vector2d& h, const Eigen::Matrix2d& H)
{
	Eigen::Matrix2d Pzz = H * Ppre * H.transpose();
	Eigen::Matrix2d Pvv = Pzz + R;
	Eigen::Matrix2d Pxz = Ppre * H.transpose();

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

// 扩展卡尔曼滤波主函数
void EKF::Filter()
{
	std::cout << "请输入采样点个数：" << std::endl;
	int num;
	std::cin >> num;

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

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

		// 非线性状态方程(非线性映射)
		Eigen::Vector2d f = Nonlinearf(i);

		// 计算状态方程雅可比矩阵(线性化矩阵)
		Eigen::Matrix2d F = JacobianMatrixF();

		// 一步预测
		Prediction(f, F);

		// 非线性测量方程
		Eigen::Vector2d h = Nonlinearh();

		// 计算测量方程雅可比矩阵(线性化矩阵)
		Eigen::Matrix2d H = JacobianMatrixH();

		// 测量更新
		Update(h, H);

		// 保存到文件
		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;

由图 1、图 2 可知，这两个状态虽然是非线性的，但是其总体趋势都呈线性变化，即非线性程度较弱，因此采用 EKF 滤波算法有望实现较好的估计结果；由图 3、图4 来看，EKF 滤波效果较好，对状态 1 和状态 2 均能很好的估计，滤波误差较小，且滤波(估计)的精度比预测的精度高。