信息融合与导航技术——扩展卡尔曼滤波算法 EKF

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

二、测量模型：

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

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

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

#pragma once
#include "Matrix.h"    // 矩阵
#include "Vector.h"    // 向量
#include      // 保存到文件
#include       // 保存到文件
#include       // 生成随机数
#include       // 生成随机数
using namespace std;

class EKF
{
public:
	EKF();
	virtual ~EKF();
	void Filter(int num);  // 扩展卡尔曼滤波主函数
	 
public:
	inline Vector getxest() const { return xest; }     // 返回状态向量估计值
	inline Vector getxreal() const { return xreal; }   // 返回状态向量真实值
	inline Vector getxpre() const { return xpre; }     // 返回状态向量预测值
	inline Vector getzreal() const { return zreal; }   // 返回测量向量值
	inline Matrix getPest() const { return Pest; }     // 返回状态向量协方差矩阵估计值
	inline Matrix getPpre() const { return Ppre; }     // 返回状态向量协方差矩阵预测值
	inline Matrix getK() const { return K; }           // 卡尔曼增益矩阵

private:
	void Initialize();             // 初始化相关参数
	void GenerateXreal(int k);     // 生成状态向量真实值
	void GenerateZreal();          // 生成测量向量值
	double getRandom();            // 生成随机数(均值为0，方差为1)
	void Nonlinearf(int k);        // 非线性状态方程(非线性映射)
	void JacobianMatrixF();        // 计算状态方程雅可比矩阵(线性化矩阵)
	void Prediction();             // 一步预测
	void Nonlinearh();             // 非线性测量方程
	void JacobianMatrixH();        // 计算测量方程雅可比矩阵(线性化矩阵)
	void Update();                 // 测量更新

private:
	Vector xreal;         // 状态向量真实值

	Vector zreal;         // 测量向量值

	Vector f;             // 非线性状态

	Matrix F;             // 状态方程雅可比矩阵(线性化矩阵)

	Matrix Q;             // 状态方程噪声矩阵

	Vector xpre;          // 状态向量预测值

	Matrix Ppre;          // 状态协方差矩阵预测值

	Vector h;             // 非线性测量

	Matrix H;             // 测量方程雅可比矩阵(线性化矩阵)

	Matrix R;             // 测量方程噪声矩阵

	Matrix K;             // 卡尔曼增益矩阵

	Vector xest;          // 状态向量估计值

	Matrix Pest;          // 状态协方差矩阵估计值

private:
	string FileName1;     // 文件名

	string FileName2;     // 文件名

	ofstream outFile1;    // 文件路径

	ofstream outFile2;    // 文件路径
};

EKF.cpp

#include "EKF.h"

// 构造函数
EKF::EKF() : FileName1("./Real.txt"), FileName2("./Filter.txt"), 
			 outFile1(FileName1, std::ios::out), outFile2(FileName2, std::ios::out)
{
	// 初始化相关参数
	Initialize();
}

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

}

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

	// 初始化测量方程噪声矩阵
	R(0, 0) = 1.0;
	R(0, 1) = 0;
	R(1, 0) = 0;
	R(1, 1) = 0.1;


	// 初始化状态协方差矩阵估计值
	Pest(0, 0) = 10.0;
	Pest(0, 1) = 0;
	Pest(1, 0) = 0;
	Pest(1, 1) = 10.0;

	// 初始化状态向量估计值
	xest(0) = 1.0;
	xest(1) = 1.0;

	// 初始化状态向量真实值
	xreal(0) = xest(0);
	xreal(1) = xest(1);

	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.Module() + sqrt(R(0, 0)) * getRandom();;
	zreal(1) = atan(xreal(0) / xreal(1)) + sqrt(R(1, 1)) * getRandom();

	return;
}

// 生成随机数(均值为0，方差为1)
double EKF::getRandom()
{
	// 每次程序运行产生不同的随机数
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();   
	std::default_random_engine gen(seed);
	std::normal_distribution<double> n(0, 1);  // 均值为0，标准差为1
	return n(gen);
}

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

	return;
}

// 计算状态方程雅可比矩阵(线性化矩阵)
void EKF::JacobianMatrixF()
{
	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;
}

// 一步预测
void EKF::Prediction()
{
	// 状态向量预测值
	xpre = f;

	// 状态协方差矩阵预测值
	Ppre = (F * Pest * (~F)) + Q;

	return;
}

// 非线性测量方程
void EKF::Nonlinearh()
{
	h(0) = xpre.Module();
	h(1) = atan(xpre(0) / xpre(1));

	return;
}

// 计算测量方程雅可比矩阵(线性化矩阵)
void EKF::JacobianMatrixH()
{
	double a = xpre.Module();
	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;
}

// 测量更新
void EKF::Update()
{
	// 单位矩阵
	Matrix I;
	I(0, 0) = 1.0;  
	I(0, 1) = 0;
	I(1, 0) = 0;  
	I(1, 1) = 1.0;

	// 卡尔曼增益矩阵
	K = (Ppre * (~H)) * (! ((H * (Ppre * (~H))) + R) );   

	// 状态向量估计值
	xest = xpre + (K * (zreal - h));

	// 状态协方差矩阵估计值
	Pest = ((I - (K * H)) * Ppre * (~(I - (K * H)))) + (K * R * (~K));

	return;
}

// 扩展卡尔曼滤波主函数
void EKF::Filter(int num)
{
	for (int i = 0; i != num; i++)
	{
		// 生成状态向量真实值
		GenerateXreal(i);

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

		// 非线性状态方程(非线性映射)
		Nonlinearf(i);

		// 计算状态方程雅可比矩阵(线性化矩阵)
		JacobianMatrixF();

		// 一步预测
		Prediction();

		// 非线性测量方程
		Nonlinearh();

		// 计算测量方程雅可比矩阵(线性化矩阵)
		JacobianMatrixH();

		// 测量更新
		Update();

		// 真实状态向量值保存到文件
		outFile1 << xreal(0) << ", " << xreal(1) << endl;

		// 状态向量估计值和状态向量预测值保存到文件
		outFile2 << xest(0) << ", " << xest(1) << ", " << xpre(0) << ", " << xpre(1) << endl;

		// 输出到控制台
		//cout<< xest(0) << ", " << xest(1) << ", " << xpre(0) << ", " << xpre(1) << endl;
	}
	return;
}

demo.cpp

#include "EKF.h"

int main()
{
	cout << "请输入滤波时间(采样点个数)：" << endl;
	int num;
	cin >> num;
	EKF ekf;
	ekf.Filter(num);
	cout << "滤波数据保存完毕，请查看" << endl;

	system("pause");
	return 0;
}

六、仿真结果：

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

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

%% 状态1
figure;
plot(1:n, real(:,1), '-', 1:n, xest(:,1), 'o-', 1:n, xest(:,3),'* -');
legend('real','est','pre');
title('状态1');
xlabel('时间/s');
grid on;

%% 状态2
figure;
plot(1:n, real(:,2), '-', 1:n, xest(:,2), 'o-', 1:n, xest(:,4),'* -');
legend('real','est','pre');
title('状态2');
xlabel('时间/s');
grid on;

%% 状态1误差
figure;
plot(1:n, abs(real(:,1)-xest(:,1)), 'o-', 1:n, abs(real(:,1)-xest(:,3)),'* -');
legend('est','pre');
title('状态1误差');
xlabel('时间/s');
grid on;

%% 状态2误差
figure;
plot(1:n, abs(real(:,2)-xest(:,2)), 'o-', 1:n, abs(real(:,2)-xest(:,4)),'* -');
legend('est','pre');
title('状态2误差');
xlabel('时间/s');
grid on;

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

(需要向量类和矩阵类源码的请私信！)
七、Matlab 源码：

clear;
clc;

%%
q1 = 0.01;
q2 = 0.1;
r1 = 1;
r2 = 0.1;
Q = diag([q1,q2]);
R = diag([r1,r2]);
n = 1000;

%%
xhat = [1; 1];
xrk = xhat;
Pest = diag([10, 10]);
xpre = [];
xr = [];
xest = [];
zr = [];

%%
for k = 1: n
        xrk = [sin(xrk(1))*xrk(2) + 0.1 * k + sqrt(q1)*randn;
        xrk(1) + cos(xrk(2)) * cos(xrk(2)) - 0.1*k + sqrt(q2)*randn];
        
        zrk = [sqrt(xrk(1) * xrk(1) + xrk(2) * xrk(2) ) + sqrt(r1)*randn;
                    atan(xrk(1) / xrk(2)) + sqrt(r2)*randn ];
                xr = [xr, xrk];
                zr = [zr, zrk];
end

for k = 1: n
        xprek = [sin(xhat(1))*xhat(2) + 0.1 * k;
        xhat(1) + cos(xhat(2)) * cos(xhat(2)) - 0.1*k];
        phi = [xhat(2)*cos(xhat(1)),   sin(xhat(1));
                   1, -sin(2*xhat(2))];
         Pprek = phi * Pest * phi' + Q;
         zpre = [sqrt(xprek(1) * xprek(1) + xprek(2) * xprek(2) );
                    atan(xprek(1) / xprek(2))];
          H = [xprek(1) / sqrt(xprek(1)*xprek(1) + xprek(2)*xprek(2)) , xprek(2) / sqrt(xprek(1)*xprek(1) + xprek(2)*xprek(2));
                  xprek(2) / (xprek(1)*xprek(1) + xprek(2)*xprek(2)),  - xprek(1) / (xprek(1)*xprek(1) + xprek(2)*xprek(2))];
           zz = zr(:,k);
           vnewk = zz - zpre;
           Pvnewk = H * Pprek * H' + R;
           Pxzk = Pprek * H';
           K = Pxzk / Pvnewk;
           xhat = xprek + K * vnewk;
           Pest = Pprek - K * Pvnewk * K';
           xpre = [xpre, xprek];
           xest = [xest, xhat];
end

%% 状态1
figure;
plot(1:n, xr(1,:), '-', 1:n, xest(1,:), 'o-', 1:n, xpre(1,:),'* -');
legend('real','est','pre');
title('状态1');
xlabel('时间/s');
grid on;

%% 状态2
figure;
plot(1:n, xr(2,:), '-', 1:n, xest(2,:), 'o-', 1:n, xpre(2,:),'* -');
legend('real','est','pre');
title('状态2');
xlabel('时间/s');
grid on;

%% 状态1误差
figure;
plot(1:n, abs(xr(1,:)-xest(1,:)), 'o-', 1:n, abs(xr(1,:)-xpre(1,:)),'* -');
legend('est','pre');
title('状态1误差');
xlabel('时间/s');
grid on;

%% 状态2误差
figure;
plot(1:n, abs(xr(2,:)-xest(2,:)), 'o-', 1:n, abs(xr(2,:)-xpre(2,:)),'* -');
legend('est','pre');
title('状态2误差');
xlabel('时间/s');
grid on;