本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分五个小节介绍:
二.卡尔曼滤波器开发实践之二: ?一个简单的位置估计卡尔曼滤波器 ? ? 也就是本文
三.卡尔曼滤波器(EKF)开发实践之三: ?基于三个传感器的海拔高度数据融合
四.卡尔曼滤波器(EKF)开发实践之四: ?ROS系统位姿估计包robot_pose_ekf详解
五.卡尔曼滤波器(EKF)开发实践之五: ?编写自己的EKF替换robot_pose_ekf中EKF滤波器
--------------------------------正文开始------------------------------------------
本文是此系列文章第二小节,将用python语言展现一个简单卡尔曼滤波器完整实现.之所以用python语言有两个原因:
一是此代码是在参考文献的作者的源码基础上改进过来的(本人也写了C++版本,合适时间再发出来);
二是python语言书写的代码表达方式,和卡尔曼五大公式在形式上很接近,非常适合新手入门.
下面将介绍在2维屏幕上模拟一个小车,跟随鼠标运动的例子.? 本例子目的重在演示卡尔曼公式代码化,故系统模型都很简单.? 系统模型分析如下:
小车的在屏幕上坐标Point(x,y),作为小车的系统状态值
和
. 很明显它是一个2维列向量:
? 即: 系统状态值个数n=2,? ?初始状态值
测量值
为当前鼠标的坐标Point(x,y), 小车在每次预测更新时,使用当前鼠标位置更新自己的位置.?
显然,测量值是一个2维列向量:
即: 测量值个数m=2
根据上一节对五大状态方程的应用分析.我们很容易得到:
1.因为在鼠标输入的测量值不参与计算时,系统状态值的变化为:?=,得:?
2.因为此例子中小车的运动没有速度和加速度控制. 所以控制矩阵和控制向量
省略不考虑.
3.关于系统不确定性协方差矩阵, 初始值(0,0)或鼠标的位置测量值均认为比较可靠. 我们设置的方差较小的初始协方差矩阵:
?4.系统噪音协方差矩阵
,我们设置一个较小值:?
5.传感器噪声协方差矩阵
:
6.状态向量到测量值向量转换矩阵
?:? 因为都是同一度量单位,不存在复杂变换关系,基本是1:1的恒等关系.得:
?综上,? 该设置的向量和矩阵,我们都有了,下面上核心基类EKF代码:
import numpy as np
class EKF(object):
"""
A abstrat class for the Extended Kalman Filter, based on the tutorial in
http://home.wlu.edu/~levys/kalman_tutorial.
"""
def __init__(self, n, m, pval=0.1, qval=1e-4, rval=0.1):
"""
Creates a KF object with n states, m observables, and specified values for
prediction noise covariance pval, process noise covariance qval, and
measurement noise covariance rval.
"""
# 预测状态变换矩阵,比如依据系统运动学方程或几何学方程得出的变换矩阵,维数n,取自状态向量个数n
# F_k = np.eye(n) # 返回的是一个二维的数组(N,N)，对角线的地方为1，其余的地方为0.
# Set up covariance matrices for process noise
self.Q_k = np.eye(n) * qval # 系统过程噪声协方差矩阵
# Current state is zero,
# F_k(n,n) * X^_k-1(n,1) --> x^_k(n,1) 新时刻状态向量
self.x = np.zeros(n) # 上一时刻(k-1)或当前时刻k的状态向量: n个元素向量,或nx1矩阵
# 协方差矩阵中间状态: P_k
# No current prediction noise covariance
self.P_current = None
# 当前时刻最优估计协方差矩阵,下一次预测和更新会复制给P_k-1
self.P_result = np.eye(n) * pval
"""
For update
"""
# 传感器自身测量噪声带来的不确定性协方差矩阵
# Set up covariance matrices for measurement noise
self.R_k = np.eye(m) * rval
# 单位矩阵I, 这里当数字1使用. P_k = P_k - K_k*H_k*P_k = (I - K_k*H_k)*P_k
# Identity matrix(单位矩阵) will be useful later
self.I = np.eye(n)
def updateR(self, R):
self.R_k = R
# 输入当前测量值, 返回最优估计值
def step(self, z): # z测量值
"""
Runs one step of the EKF on observations z, where z is a tuple of length M.
Returns a NumPy array representing the updated state.
"""
# Predict ----------------------------------------------------
# 根据上一时刻系统状态值X_k-1,预测当前时刻X_k. 主要完成方程(1)
# (1). X_k = F_k * X_k-1 + B_k * u_k
# F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
# B_k: 系统外部控制矩阵 维数确保可以和前面F_k相乘
# u_k: 系统外部控制向量,例如加速度,转向等. 维数确保可以和前面F_k相乘
self.x, F_k = self.stateTransitionFunction(self.x)
# 根据上一时刻协方差矩阵P_k-1,预测当前时刻协方差矩阵P_k. 主要完成方程(2)
# (2). P_k = F_k * P_k-1 * F_k^T + Q_k
# Q_k: 各状态量对应的噪音协方差矩阵nxn
self.P_current = F_k * self.P_result * F_k.T + self.Q_k # P_k
# Update -----------------------------------------------------
# 返回状态值到测量值的变换矩阵,把系统状态量转换为与测量值向量同样的单位或度量,
# 必要时需要非线性函数线性化(返回雅各比矩阵)
# 1. 经雅各比矩阵H_k * 当前状态值向量得 -> 到的预估测量值向量:zz_k(mx1)
# H_k: 预测状态值转测量值变换函数的雅各比矩阵
zz_k, H_k = self.stateToMeasurementTransitionFunction(self.x)
# 卡尔曼增益: K_k
# (3). K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^-1
# R_k: 传感器噪声(不确定性)协方差矩阵mxm
K_k = np.dot(self.P_current.dot(H_k.T), np.linalg.inv(H_k.dot(self.P_current).dot(H_k.T) + self.R_k))
# 最终,最优预测状态向量值
# z_k: 传感器读数或均值
# (4). X^_k = X_k + K_k * (z_k - H_k * X_k)
# self.x += np.dot(K_k, (np.array(z) - H_k.dot(self.x)))
# zz_k: 由状态转测量值函数stateToMeasurementTransitionFunction返回的值
# (4). X^_k = X_k + K_k * (z_k - zz_k) # Note: 此处的zz_k = H_k * X_k)
self.x += np.dot(K_k, (np.array(z) - zz_k))
# 最后,最优预测协方差矩阵
# (5). P^_k = P_k - K_k * H_k * P_k = (I - K_k * H_k) * P_k
self.P_result = np.dot(self.I - np.dot(K_k, H_k), self.P_current)
# return self.x.asarray()
return self.x
"""
根据系统运动学方程或几何学方程,更新预测状态向量(nx1)
# (1). X_k = F_k * X_k-1 + B_k * u_k
# F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
# B_k: 系统外部控制矩阵
# u_k: 系统外部控制向量,例如加速度,转向等.
# 这里注意: B_k * u_k的结果维数要保持和X_k同样的维数,以便做矩阵相加
参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
返回:
X_k: 依据方程(1)计算的含有n个元素的新的(k)预测状态向量(nx1)
F_k: 一个依据系统运动学方程或几何学方程设计的预测状态变换模型(n*n矩阵)
"""
@abstractmethod
def stateTransitionFunction(self, x):
raise NotImplementedError()
"""
预测状态值变换函数,把预测状态值向量转换为与测量值向量同样的单位或度量,
必要时需要非线性函数线性化(返回雅各比矩阵)
方程(4). X^_k = X_k + K_k * (z_k - zz) # zz = H_k * X_k
参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
返回:
1. 预估测量值向量ZZ_K: 一个m个元素的预估测量值向量: 经 经雅各比矩阵 * 当前状态值向量 得到的 "预估测量值向量"
2. H_k: 一个m*n雅各比矩阵H_k,矩阵的元素为: 状态值转换为观测值的非线性函数的一阶导数,或有限差分, 或连续差分的比值
m为测量值个数, n为状态量个数, 用处1: H_k(mxn) * X(nx1) = ZZ_k(mx1),以便下一步方程(4)执行: Z_k - ZZ_K
"""
@abstractmethod
def stateToMeasurementTransitionFunction(self, x):
raise NotImplementedError()?上面是本文简单卡尔曼滤波器的核心基类 EKF代码. 为了根据实际项目实现具体的功能,我们需要从EKF继承一个之类TrackerEKF,用来实现基类EKF中的两个抽象方法:
import numpy as np
import EKF
class TrackerEKF(EKF):
def __init__(self):
EKF.__init__(self, 2, 2) # 状态值个数: n=2, 测量值个数: m=2
# 返回: X_k, F_k
def stateTransitionFunction(self, x):
# State-transition function is identity
# X_k这里直接返回了当前状态值X_k-1的相同值
return np.copy(x), np.eye(2) # 返回的是一个nxn矩阵，对角线的地方为1，其余的地方为0.
# 返回: 预估测量值向量ZZ_K, H_k
def stateToMeasurementTransitionFunction(self, x):
H_k = np.eye(2) # 状态值转换为测量值的函数为: y= f(x) = x,基本是恒等关系,故返回2x2单位矩阵
return H_k.dot(x), H_k # 返回经状态转换函数变换后的测量值ZZ_k(mx1) = H_k(mxn) * X(nx1)?如上: 重要的代码实现和分析,都在代码中做了注释.
上面只是滤波器部分代码. 实际运行需要基于openCV在图形界面操作.相关代码稍后附上,您也可以参考原作者在github上的源码见参考文献. 程序运行效果图:
?蓝色轨迹为鼠标移动轨迹, 绿色轨迹为模拟小车以鼠标位置为测量值的最优评估轨迹.
参考文献:
本文源码基于The Extended Kalman Filter: An Interactive Tutorial for Non-Experts?的作者放在github上的TinyEKF改进而来. 再次特表感谢!