青岛 公司 网站建设,wordpress api,临海网站制作,如何做自己的网站商城站1.filterpy
FilterPy是一个实现了各种滤波器的Python模块#xff0c;它实现著名的卡尔曼滤波和粒子滤波器。我们可以直接调用该库完成卡尔曼滤波器实现。其中的主要模块包括#xff1a; filterpy.kalman 该模块主要实现了各种卡尔曼滤波器#xff0c;包括常见的线性卡尔曼滤…1.filterpy
FilterPy是一个实现了各种滤波器的Python模块它实现著名的卡尔曼滤波和粒子滤波器。我们可以直接调用该库完成卡尔曼滤波器实现。其中的主要模块包括 filterpy.kalman 该模块主要实现了各种卡尔曼滤波器包括常见的线性卡尔曼滤波器扩展卡尔曼滤波器等。 filterpy.common 该模块主要提供支持实现滤波的各种辅助函数其中计算噪声矩阵的函数线性方程离散化的函数等。 filterpy.stats 该模块提供与滤波相关的统计函数包括多元高斯算法对数似然算法PDF及协方差等。 filterpy.monte_carlo 该模块提供了马尔科夫链蒙特卡洛算法主要用于粒子滤波。
开源代码在
https://github.com/rlabbe/filterpy/tree/master/filterpy/kalman
我们介绍下卡尔曼滤波器的实现主要分为预测和更新两个阶段在进行滤波之前需要先进行初始化
初始化
预先设定状态变量dim_x和观测变量维度dim_z、协方差矩阵P、运动形式和观测矩阵H等一般各个协方差矩阵都会初始化为单位矩阵根据特定的场景需要相应的设置。 def __init__(self, dim_x, dim_z, dim_u 0, x None, P None,Q None, B None, F None, H None, R None):Kalman FilterRefer to http:/github.com/rlabbe/filterpyMethod-----------------------------------------Predict | Update-----------------------------------------| K PH^T(HPH^T R)^-1x Fx Bu | y z - HxP FPF^T Q | x x Ky| P (1 - KH)P| S HPH^T R-----------------------------------------note: In update unit, here is a more numerically stable way: P (I-KH)P(I-KH) KRKParameters----------dim_x: intdims of state variables, eg:(x,y,vx,vy)-4dim_z: intdims of observation variables, eg:(x,y)-2dim_u: intdims of control variables,eg: a-1p p vt 0.5at^2v v at[p;v] [1,t;0,1][p;v] [0.5t^2;t]aassert dim_x 1, dim_x must be 1 or greaterassert dim_z 1, dim_z must be 1 or greaterassert dim_u 0, dim_u must be 0 or greaterself.dim_x dim_xself.dim_z dim_zself.dim_u dim_u# initialization# predictself.x np.zeros((dim_x, 1)) if x is None else x # stateself.P np.eye(dim_x) if P is None else P # uncertainty covarianceself.Q np.eye(dim_x) if Q is None else Q # process uncertainty for predictionself.B None if B is None else B # control transition matrixself.F np.eye(dim_x) if F is None else F # state transition matrix# updateself.H np.zeros((dim_z, dim_x)) if H is None else H # Measurement function zHxself.R np.eye(dim_z) if R is None else R # observation uncertaintyself._alpha_sq 1. # fading memory controlself.z np.array([[None] * self.dim_z]).T # observationself.K np.zeros((dim_x, dim_z)) # kalman gainself.y np.zeros((dim_z, 1)) # estimation errorself.S np.zeros((dim_z, dim_z)) # system uncertainty, S HPH^T Rself.SI np.zeros((dim_z, dim_z)) # inverse system uncertainty, SI S^-1self.inv np.linalg.invself._mahalanobis None # Mahalanobis distance of measurementself.latest_state init # last process name
预测阶段
接下来进入预测环节为了保证通用性引入了遗忘系数α其作用在于调节对过往信息的依赖程度α越大对历史信息的依赖越小 代码如下 def predict(self, u None, B None, F None, Q None):Predict next state (prior) using the Kalman filter state propagation equations:x Fx BuP fading_memory*FPF^T QParameters----------u : ndarrayOptional control vector. If not None, it is multiplied by Bto create the control input into the system.B : ndarray of (dim_x, dim_z), or NoneOptional control transition matrix; a value of Nonewill cause the filter to use self.B.F : ndarray of (dim_x, dim_x), or NoneOptional state transition matrix; a value of Nonewill cause the filter to use self.F.Q : ndarray of (dim_x, dim_x), scalar, or NoneOptional process noise matrix; a value of None will cause thefilter to use self.Q.if B is None:B self.Bif F is None:F self.Fif Q is None:Q self.Qelif np.isscalar(Q):Q np.eye(self.dim_x) * Q# x Fx Buif B is not None and u is not None:self.x F self.x B uelse:self.x F self.x# P fading_memory*FPF Qself.P self._alpha_sq * (F self.P F.T) Qself.latest_state predict
更新阶段
按下式进行状态的更新 也可以写为 其中y是测量余量S是测量余量的协方差矩阵。
在实际应用中会做一些微调使协方差矩阵为 代码如下 def update(self, z, R None, H None):Update Process, add a new measurement (z) to the Kalman filter.K PH^T(HPH^T R)^-1y z - Hxx x KyP (1 - KH)P or P (I-KH)P(I-KH) KRKIf z is None, nothing is computed.Parameters----------z : (dim_z, 1): array_likemeasurement for this update. z can be a scalar if dim_z is 1,otherwise it must be convertible to a column vector.R : ndarray, scalar, or NoneOptionally provide R to override the measurement noise for thisone call, otherwise self.R will be used.H : ndarray, or NoneOptionally provide H to override the measurement function for thisone call, otherwise self.H will be used.if z is None:self.z np.array([[None] * self.dim_z]).Tself.y np.zeros((self.dim_z, 1))returnz reshape_z(z, self.dim_z, self.x.ndim)if R is None:R self.Relif np.isscalar(R):R np.eye(self.dim_z) * Rif H is None:H self.Hif self.latest_state predict:# common subexpression for speedPHT self.P H.T# S HPH R# project system uncertainty into measurement spaceself.S H PHT Rself.SI self.inv(self.S)# K PHinv(S)# map system uncertainty into kalman gainself.K PHT self.SI# P (I-KH)P(I-KH) KRK# This is more numerically stable and works for non-optimal K vs# the equation P (I-KH)P usually seen in the literature.I_KH np.eye(self.dim_x) - self.K Hself.P I_KH self.P I_KH.T self.K R self.K.T# y z - Hx# error (residual) between measurement and predictionself.y z - H self.xself._mahalanobis math.sqrt(float(self.y.T self.SI self.y))# x x Ky# predict new x with residual scaled by the kalman gainself.x self.x self.K self.yself.latest_state update
那接下来我们就是用filterpy中的卡尔曼滤波器方法完成小车位置的预测。
2.小车案例
现在利用卡尔曼滤波对小车的运动状态进行预测。主要流程如下所示
导入相应的工具包小车运动数据生成参数初始化利用卡尔曼滤波进行小车状态预测可视化观察参数的变化与结果
下面我们看下整个流程实现
导入包
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
from filterpy.kalman import KalmanFilter
小车运动数据生成
在这里我们假设小车作速度为1的匀速运动
# 生成1000个位置从1到1000是小车的实际位置
z np.linspace(1,1000,1000)
# 添加噪声
mu,sigma 0,1
noise np.random.normal(mu,sigma,1000)
# 小车位置的观测值
z_nosie znoise
参数初始化
# dim_x 状态向量size,在该例中为[p,v]即位置和速度,size2
# dim_z 测量向量size假设小车为匀速速度为1测量向量只观测位置size1
my_filter KalmanFilter(dim_x2, dim_z1)# 定义卡尔曼滤波中所需的参数
# x 初始状态为[0,0],即初始位置为0速度为0.
# 这个初始值不是非常重要在利用观测值进行更新迭代后会接近于真实值
my_filter.x np.array([[0.], [0.]])# p 协方差矩阵表示状态向量内位置与速度的相关性
# 假设速度与位置没关系协方差矩阵为[[1,0],[0,1]]
my_filter.P np.array([[1., 0.], [0., 1.]])# F 初始的状态转移矩阵假设为匀速运动模型可将其设为如下所示
my_filter.F np.array([[1., 1.], [0., 1.]])# Q 状态转移协方差矩阵也就是外界噪声
# 在该例中假设小车匀速外界干扰小所以我们对F非常确定觉得F一定不会出错所以Q设的很小
my_filter.Q np.array([[0.0001, 0.], [0., 0.0001]])# 观测矩阵 Hx p
# 利用观测数据对预测进行更新观测矩阵的左边一项不能设置成0
my_filter.H np.array([[1, 0]])
# R 测量噪声方差为1
my_filter.R 1
卡尔曼滤波进行预测
# 保存卡尔曼滤波过程中的位置和速度
z_new_list []
v_new_list []
# 对于每一个观测值进行一次卡尔曼滤波
for k in range(len(z_nosie)):# 预测过程 my_filter.predict()# 利用观测值进行更新my_filter.update(z_nosie[k])# do something with the outputx my_filter.x# 收集卡尔曼滤波后的速度和位置信息z_new_list.append(x[0][0])v_new_list.append(x[1][0]) 可视化 预测误差的可视化 # 位移的偏差
dif_list []
for k in range(len(z)):dif_list.append(z_new_list[k]-z[k])
# 速度的偏差
v_dif_list []
for k in range(len(z)):v_dif_list.append(v_new_list[k]-1)
plt.figure(figsize(20,9))
plt.subplot(1,2,1)
plt.xlim(-50,1050)
plt.ylim(-3.0,3.0)
plt.scatter(range(len(z)),dif_list,color b,label 位置偏差)
plt.scatter(range(len(z)),v_dif_list,color y,label 速度偏差)
plt.legend() 运行结果如下所示 2.卡尔曼滤波器参数的变化
首先定义方法将卡尔曼滤波器的参数堆叠成一个矩阵右下角补0我们看一下参数的变化。
# 定义一个方法将卡尔曼滤波器的参数堆叠成一个矩阵右下角补0
def filter_comb(p, f, q, h, r):a np.hstack((p, f))b np.array([r, 0])b np.vstack([h, b])b np.hstack((q, b))a np.vstack((a, b))return a 对参数变化进行可视化
# 保存卡尔曼滤波过程中的位置和速度
z_new_list []
v_new_list []
# 对于每一个观测值进行一次卡尔曼滤波
for k in range(1):# 预测过程 my_filter.predict()# 利用观测值进行更新my_filter.update(z_nosie[k])# do something with the outputx my_filter.xc filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)plt.figure(figsize(32,18))sns.set(font_scale4)#sns.heatmap(c,squareTrue,annotTrue,xticklabelsFalse,yticklabelsFalse,cbarFalse)sns.heatmap(c,squareTrue,annotTrue,xticklabelsFalse,yticklabelsFalse,cbarFalse)
对比变换 从图中可以看出变化的P其他的参数FQH,R为变换。另外状态变量x和卡尔曼系数K也是变化的。
3.概率密度函数
为了验证卡尔曼滤波的结果优于测量的结果绘制预测结果误差和测量误差的概率密度函数
# 生成概率密度图像
z_noise_list_std np.std(noise)
z_noise_list_avg np.mean(noise)
z_filterd_list_std np.std(dif_list)
import seaborn as sns
plt.figure(figsize(16,9))
ax sns.kdeplot(noise,shadeTrue,colorr,labelstd%.3f%z_noise_list_std)
ax sns.kdeplot(dif_list,shadeTrue,colorg,labelstd%.3f%z_filterd_list_std)
结果如下 总结
1.了解filterpy工具包
FilterPy是一个实现了各种滤波器的Python模块它实现著名的卡尔曼滤波和粒子滤波器。直接调用该库完成卡尔曼滤波器实现。
2.知道卡尔曼滤波的实现过程
卡尔曼滤波器的实现主要分为预测和更新两个阶段在进行滤波之前需要先进行初始化
初始化
预先设定状态变量和观测变量维度、协方差矩阵、运动形式和转换矩阵
预测
对状态变量X和协方差P进行预测
更新
利用观测结果对卡尔曼滤波的结果进行修征
3.能够利用卡尔曼滤波器完成小车目标状态的预测 导入相应的工具包 小车运动数据生成匀速运动的小车模型 参数初始化对卡尔曼滤波的参数进行初始化包括状态变量和观测变量维度、协方差矩阵、运动形式和转换矩阵等 利用卡尔曼滤波进行小车状态预测使用Filterpy工具包调用predict和update完成小车状态的预测 可视化观察参数的变化与结果 1.预测误差的分布p,v 2.参数的变化参数中变化的是XPK不变的是FQHR 误差的概率密度函数卡尔曼预测的结果优于测量结果
