扩展卡尔曼滤波的仿真案例，参考书为北航宇航学院王可东老师的Kalman滤波基础及Matlab仿真
一、状态模型：
二、测量模型：
状态方程和测量方程中的噪声均为期望为零的白噪声。
三、状态模型和测量模型的线性化(Jacobian矩阵)：
四、状态模型和测量模型的噪声矩阵及初始状态及协方差矩阵：
五、C++ 仿真源码：
EKF.h
#pragma once
#include "Matrix.h" // 矩阵
#include "Vector.h" // 向量
#include <fstream> // 保存到文件
#include <string> // 保存到文件
#include <random> // 生成随机数
#include <chrono> // 生成随机数
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 均能很好的估计，滤波误差较小，且滤波(估计)的精度比预测的精度高。
(需要向量类和矩阵类源码的请私信！)
