本文使用 Zhihu On VSCode 创作并发布
0 前言
扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。
EKF 的基本思想是利用泰勒级数展开将非线性系统的状态转移函数
1 贝叶斯滤波的三大概率密度函数
在此前的文章
石鹏：从概率到贝叶斯滤波zhuanlan.zhihu.com
和
石鹏：从贝叶斯滤波到卡尔曼滤波zhuanlan.zhihu.com
中，已经讲到贝叶斯滤波的先验概率密度函数、似然概率密度函数和后验概率密度函数：
(1) 先验概率密度函数
(2) 似然概率密度函数
(3) 后验概率密度函数
其中，后验概率密度函数中的归一化常数
2 扩展卡尔曼滤波的假设
扩展卡尔曼滤波以贝叶斯滤波为理论基础，并作了五个前提假设。
2.1 与卡尔曼滤波相同的假设
(1) 假设一：状态量服从正态分布
(2) 假设二：观测量服从正态分布
(3) 假设三：过程噪声服从均值为 0 的正态分布
(4) 假设四：观测噪声服从均值为 0 的正态分布
2.2 与卡尔曼滤波不同的假设
(5) 假设五：状态转移函数和（或）观测函数为非线性函数
在卡尔曼滤波的前提假设中，认为状态方程中的状态转移函数
不同于标准卡尔曼滤波，扩展卡尔曼滤波处理的是非线性系统，假设系统的状态转移函数和（或）观测函数为非线性函数。
3 扩展卡尔曼滤波的公式推导
3.1 预测步的两个公式
当状态转移函数为线性函数时，扩展卡尔曼滤波的预测步与标准卡尔曼滤波相同；当状态转移函数为非线性函数时，扩展卡尔曼滤波的预测步采用下面的推导过程。
根据假设一，
对状态转移函数
其中，
令
结合假设三与上式，
类似推导标准卡尔曼滤波时的做法，使用 Mathematica 软件做符号推导，整理表达式结果可知，先验概率密度函数
其中，
3.2 更新步的三个公式
当观测函数为线性函数时，扩展卡尔曼滤波的更新步与标准卡尔曼滤波相同；当观测函数为非线性函数时，扩展卡尔曼滤波的更新步采用下面的推导过程。
对观测函数
其中，
令
结合假设四、公式 (3.1)、公式 (3.2) 及
其中，归一化常数
类似推导标准卡尔曼滤波时的做法，使用 Mathematica 软件做符号推导，整理表达式结果可知，后验概率密度函数
其中，
4 矩阵形式的扩展卡尔曼滤波
上文内容所描述的是一维的扩展卡尔曼滤波，当状态量和观测量不再是单一的随机变量而是由多个随机变量组成的序列时，扩展卡尔曼滤波中各个量的维数也将随之改变：
- 状态量
由随机变量演变为随机向量，随机向量中的每一个分量为一个状态量随机变量。维数为
- 状态转移比例项
雅可比矩阵，维数为
演变为
。此时，
是由每个状态转移函数对每个状态量分量求偏导后，将上一时刻状态量的后验估计代入得到：
- 状态量概率密度函数均值
演变为矩阵，维数为
- 状态量概率密度函数方差
演变为协方差矩阵，用
表示，维数为
- 过程噪声方差
演变为协方差矩阵，用
表示，维数为
- 观测量
由随机变量演变为随机向量，随机向量中的每一个分量为一个观测量随机变量。维数为
- 观测值
由单一值演变为由单一值组成的值矩阵，维数为
- 观测比例项
雅可比矩阵，维数为
演变为
。此时，
是由每个观测函数对每个观测量分量求偏导后，将当前时刻状态量的先验估计代入得到：
- 观测噪声方差
演变为协方差矩阵，用
表示，维数为
- 卡尔曼增益系数
演变为矩阵，维数为
对应的五个公式演变为：
公式 (4.3) 中
当然，对于某个非线性系统，不一定状态转移和观测都是非线性的：
- 线性的状态转移 + 非线性的观测
此时，滤波递推公式由卡尔曼滤波的预测步两公式和扩展卡尔曼滤波的更新步三公式组成。 - 非线性的状态转移 + 线性的观测
此时，滤波递推公式由扩展卡尔曼滤波的预测步两公式和卡尔曼滤波的更新步三公式组成。
5 应用实例——基于毫米波雷达与扩展卡尔曼滤波的目标跟踪
5.1 系统分析
毫米波雷达（Radar）与激光雷达的的检测原理不同。激光雷达利用光的直线传播原理获得目标在笛卡尔坐标系下的距离信息；毫米波雷达利用电磁波的多普勒效应，获得目标在极坐标系下的距离
假设我们使用毫米波雷达对某作匀速直线运动的目标在笛卡尔坐标系内的横坐标
观测量可设为：
故，毫米波雷达的测量数据特性可用下图进行表征。
其中，距离变化率
故，观测函数
此时，滤波递推公式由卡尔曼滤波的预测步两公式和扩展卡尔曼滤波的更新步三公式组成：
卡尔曼滤波的预测步两公式
扩展卡尔曼滤波的更新步三公式
由于目标作匀速直线运动，故公式 (5.1) 中的控制项
5.2 雅可比矩阵求解
公式 (5.4) 和公式 (5.5) 中的雅可比矩阵
直接求解
jacobian() 进行求解。创建 matlab 脚本，命名为
CalculateHj.m 并输入如下内容后保存：
% define symbol variables
syms px py vx vy
% state random vector
X = [px py vx vy];
% measurement functions
h1 = sqrt(px^2 + py^2);
h2 = atan(py / px);
h3 = (px * vx + py * vy) / sqrt(px^2 + py^2);
% measurement jacobian matrix
Hj = jacobian([h1 h2 h3], X)
运行 CalculateHj.m 脚本，得到
Hj =
[ px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2), 0, 0]
[ -py/(px^2*(py^2/px^2 + 1)), 1/(px*(py^2/px^2 + 1)), 0, 0]
[ vx/(px^2 + py^2)^(1/2) - (px*(px*vx + py*vy))/(px^2 + py^2)^(3/2), vy/(px^2 + py^2)^(1/2) - (py*(px*vx + py*vy))/(px^2 + py^2)^(3/2), px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2)]
可以直接使用上述结果，也可以对其进行化简：
5.3 代码
代码与此前的文章
（十三）手把手教你写卡尔曼滤波器blog.shipengx.com
相似，区别在于状态量和观测量有所不同，且使用雅可比矩阵
ExtendedKalmanFilter.h
#ifndef EXTENDED_KALMAN_FILTER_
#define EXTENDED_KALMAN_FILTER_
#include "D:/eigen3/Eigen/Dense"
class ExtendedKalmanFilter
{
private:
bool is_inited; // flag of initialization
bool just_begin_filt; // flag of just begining filt data
Eigen::VectorXd X; // state vector
Eigen::MatrixXd F; // state transition matrix
Eigen::MatrixXd P; // state covariance matrix
Eigen::MatrixXd Q; // process noise covariance matrix
Eigen::MatrixXd Hj; // measurement jacobian matrix
Eigen::MatrixXd R; // measurement noise covariance matrix
Eigen::MatrixXd K; // extended kalman gain coefficient
uint64_t timestamp_last; // timestamp of last frame: us
float dt; // delta time: s
public:
Eigen::VectorXd Z; // measurement vector
uint64_t timestamp_now; // timestamp of current frame: us
public:
ExtendedKalmanFilter();
~ExtendedKalmanFilter();
inline bool &IsInited()
{
return is_inited;
}
void Init();
inline void Reset()
{
is_inited = false;
}
inline void SetF(const Eigen::MatrixXd &f)
{
F = f;
}
inline void SetF()
{
F << 1.0f, 0.0f, dt, 0.0f,
0.0f, 1.0f, 0.0f, dt,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f;
}
inline void SetP(const Eigen::MatrixXd &p)
{
P = p;
}
inline void SetQ(const Eigen::MatrixXd &q)
{
Q = q;
}
inline void SetHj(const Eigen::MatrixXd &hj)
{
Hj = hj;
}
inline void SetR(const Eigen::MatrixXd &r)
{
R = r;
}
/**
* @brief Predict step.
*
*/
void Predict();
/**
* @brief Update step.
*
*/
void Update();
};
/**
* @brief To update measurement vector and timestamp.
*
* @param Z0 Z(0)
* @param Z1 Z(1)
* @param Z2 Z(2)
* @param timestamp Current timestamp.
*/
void RecvRawData(float &Z0, float &Z1, float &Z2, uint32_t ×tamp)
{
// Z0 = rho;
// Z1 = phi;
// Z2 = rho_dot;
// timestamp = timestamp_new;
}
#endif
ExtendedKalmanFilter.cpp
#include "math.h"
#include "ExtendedKalmanFilter.h"
/**
* @brief Construct a new Extended Kalman Filter:: Extended Kalman Filter object
*
*/
ExtendedKalmanFilter::ExtendedKalmanFilter()
: is_inited(false),
just_begin_filt(false),
X(4),
F(4, 4),
P(4, 4),
Q(4, 4),
Z(3),
Hj(3, 4),
R(3, 3),
K(4, 3),
timestamp_now(0),
timestamp_last(0),
dt(0.0f)
{
}
/**
* @brief Destroy the Extended Kalman Filter:: Extended Kalman Filter object
*
*/
ExtendedKalmanFilter::~ExtendedKalmanFilter() {}
/**
* @brief Initialize the extended kalman filter.
*
*/
void ExtendedKalmanFilter::Init()
{
if (!IsInited())
{
X << Z(0) * cos(Z(1)),
Z(0) * sin(Z(1)),
Z(2) * cos(Z(1)),
Z(2) * sin(Z(1));
P << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 10.0, 0.0,
0.0, 0.0, 0.0, 10.0;
Q << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
R << 0.09, 0.0, 0.0,
0.0, 0.009, 0.0,
0.0, 0.0, 0.09;
timestamp_last = timestamp_now;
is_inited = true;
just_begin_filt = true;
}
else
just_begin_filt = false;
}
/**
* @brief Predict step.
*
*/
void ExtendedKalmanFilter::Predict()
{
if (just_begin_filt)
return;
dt = (timestamp_now - timestamp_last) / 1E6;
SetF();
timestamp_last = timestamp_now;
// predict state vector
X = F * X;
// predict state covariance matrix
P = F * P * F.transpose() + Q;
}
/**
* @brief Update step.
*
*/
void ExtendedKalmanFilter::Update()
{
if (just_begin_filt)
return;
float c1 = X(0) ^ 2 + X(1) ^ 2;
float c2 = sqrt(c1);
float c3 = c1 * c2;
if (fabs(c1 > 0.0001))
{
Hj << X(0) / c2, X(1) / c2, 0, 0,
-X(1) / c2, X(0) / c2, 0, 0,
X(1) * (X(2) * X(1) - X(3) * X(0)) / c3, X(0) * (X(3) * X(0) - X(2) * X(1)) / c3, X(0) / c2, X(1) / c2;
}
Eigen::VectorXd(3) hx;
hx << c2, atan2(X(1), X(0)), (X(0) * X(2) + X(1) * X(3)) / c2;
static Eigen::MatrixXd I = Eigen::MatrixXd::Identity(X.size(), X.size());
// calculate extended kalman gain
K = P * Hj.transpose() * (Hj * P * Hj.transpose() + R).inverse();
// update state vector
X = X + K * (Z - hx);
// update state covariance matrix
P = (I - K * Hj) * P;
}
main.cpp
#include "ExtendedKalmanFilter.h"
#include <iostream>
int main()
{
ExtendedKalmanFilter ekf;
while (1)
{
RecvRawData(ekf.Z(0), ekf.Z(1), ekf.Z(2), ekf.timestamp_now);
Init();
Predict();
Update();
}
return 0;
}
点击这里下载完整工程。
6 扩展卡尔曼滤波器的优缺点
优点
扩展卡尔曼滤波与标准卡尔曼滤波有着相似的计算形式，因此运算速度同样很快。
缺点
扩展卡尔曼滤波对非线性的状态转移函数和（或）观测函数使用一阶泰勒级数展开作了线性近似，忽略了二阶及以上的高阶项，因此精度一般（通常称为一阶精度），对于高度非线性问题效果较差。此外，雅可比矩阵的计算较为繁琐，容易出错。
总结
扩展卡尔曼滤波所应用的线性近似是否具有优势主要取决于两个因素：被近似的局部非线性化程度和不确定程度。
7 参考
- b站忠实的王大头《贝叶斯滤波与卡尔曼滤波》第十三讲：扩展卡尔曼滤波及其代码
- 无人驾驶技术入门（十八）| 手把手教你写扩展卡尔曼滤波器
- 百度百科 - 泰勒公式
如果觉得文章对你有所帮助的话，欢迎点赞/评论/收藏/关注，相互交流，共同进步~
扫一扫
1321
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
