原理:
在程序设计中,渐消因子的初值设为1。即:和1.7的表达式相同。
渐消滤波算例分析:
历元间隔1s;状态向量维度为3,分别是位置、速度、加速度;每个历元观测向量维度为2,分别是位置和速度。
滤波初值设置:
位置 |
0m |
速度 |
5m/s |
加速度 |
0m/s2 |
状态向量 |
[0,5,0] |
状态转移矩阵 |
[[1,dt,0.5dt2],[0,1,dt],[0,0,1]] |
设计矩阵 |
[[1,0,0], [0,1,0]] |
状态向量协方差阵 |
单位阵(3*3) |
观测噪声协方差阵 |
[[100,0], [0,0.01]] |
过程噪声矩阵 |
[[dt5/20, dt4/8, dt3/6], [dt4/8, dt3/3, dt2/2], [dt3/6, dt2/2, dt]] |
假设载体在X轴方向运动,0-200s载体以5m/s的速度匀速运动,200-400s载体以0.05m/s2匀加速运动,400-600s以-0.02 m/s2匀减速运动,600-800s匀速运动,800-1000s停止状态。直接在轨迹发生器的位置和速度输出上叠加白噪声信号来模拟X轴方向上的位置和速度。
标准Kalman滤波
渐消Kalman滤波
在动力学模型误差较小的情况下,标准Kalman滤波比渐消Kalman要好很多。如果错误地将动力学模型建立错误,会是怎样地结果?
假设动力学模型中状态转移矩阵变为:
[[1,dt,0.5dt2],[0,3,dt],[0,0,1]]
标准Kalman滤波(错误动力学模型)
渐消Kalman滤波(错误动力学模型)
可以看到,采用渐消滤波位置不发散了,位置误差在25m以内。
历元间隔1s;状态向量维度为1,位置;每个历元观测向量维度为1,位置。
滤波初值设置:
位置 |
0m |
状态转移矩阵 |
[1] |
设计矩阵 |
[1] |
状态向量协方差阵 |
单位阵(1*1) |
观测噪声协方差阵 |
[100] |
过程噪声矩阵 |
[0.01] |
0-400s载体以0.05m/s2进行匀加速运动。
标准Kalman滤波
渐消Kalman滤波
采用渐消Kalman滤波,误差大部分历元在20m以内,当动力学模型不准确的时候,采用渐消滤波可以有效抑制发散。
Python代码:
class FadingKalmanFilter(object):
def __init__(self, alpha, dim_x, dim_z, dim_u=0):
assert alpha >= 1
assert dim_x > 0
assert dim_z > 0
assert dim_u >= 0
self.alpha_ = alpha # 渐消因子
self.dim_x = dim_x # 状态值个数
self.dim_z = dim_z # 观测值个数
self.dim_u = dim_u # 控制值个数
self.x = zeros((dim_x, 1)) # Current state estimate 当前历元状态参数向量(dim_x*1)
self.P = eye(dim_x) # Current state covariance matrix 当前状态参数向量协方差阵(dim_x*dim_x)
self.Q = eye(dim_x) # Process noise covariance matrix 过程噪声协方差阵(dim_x*dim_x)
self.B = 0. # control transition matrix 控制矩阵(一般不考虑)
self.F = np.eye(dim_x) # state transition matrix 状态转移矩阵(dim_x*dim_x)
self.H = zeros((dim_z, dim_x)) # Measurement function 观测设计矩阵(dim_z*dim_x)
self.R = eye(dim_z) # Measurement noise covariance matrix 观测噪声协方差矩阵(dim_z*dim_z)
self.z = np.array([[None] * dim_z]).T
# gain and residual are computed during the innovation step. We
# save them so that in case you want to inspect them for various
# purposes
self.K = 0 # kalman gain 增益矩阵
self.y = zeros((dim_z, 1)) # residual are computed during the innovation step 新息向量
self.S = np.zeros((dim_z, dim_z)) # system uncertainty (measurement space)
self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty
#self._alpha_sq = 1. # fading memory control
# identity matrix. Do not alter this.
self._I = np.eye(dim_x)
# Only computed only if requested via property 似然函数
self._log_likelihood = log(sys.float_info.min)
self._likelihood = sys.float_info.min
self._mahalanobis = None
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy() # 当前状态向量预测值
self.P_prior = self.P.copy() # 当前状态向量预测协方差阵
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy() # 当前状态向量估值
self.P_post = self.P.copy() # 当前状态向量估值的协方差阵
self.inv = np.linalg.inv
def settingalpha_(self,z, u=None, B=None, F=None, Q=None,H=None,P=None,R=None):
#计算预测状态向量
if B is None:
B = self.B
if P is None:
P = self.P
if R is None:
R = self.R
if H is None:
H = self.H
if F is None:
F = self.F
if Q is None:
Q = self.Q
elif isscalar(Q): # 如果Q是标量
Q = eye(self.dim_x) * Q
# x = Fx + Bu
if B is not None and u is not None:
x = dot(F, self.x) + dot(B, u)
else:
x = dot(F, self.x)
#计算预测残差向量
y= z - dot(H, self.x)
#计算预测残差向量的协方差阵
S=self.alpha_*dot(y,y.T)/(1+self.alpha_)
HF=dot(H,F)
HFP=dot(HF,P)
M=dot(HFP,HF.T)
HQ=dot(H,Q)
N=S-dot(HQ,H.T)-R
alpha=np.trace(N)/np.trace(M)
if alpha>=1:
self.alpha_=alpha
else:
self.alpha_=1.
pass
# 预测
def predict(self, u=None, B=None, F=None, Q=None):
"""
Predict next state (prior) using the Kalman filter state propagation
equations.
Parameters
----------
u : np.array, default 0
Optional control vector.控制向量(一般不用)
B : np.array(dim_x, dim_u), or None
Optional control transition matrix; a value of None
will cause the filter to use `self.B`.
F : np.array(dim_x, dim_x), or None
Optional state transition matrix; a value of None
will cause the filter to use `self.F`.
Q : np.array(dim_x, dim_x), scalar, or None
Optional process noise matrix; a value of None will cause the
filter to use `self.Q`.
"""
if B is None:
B = self.B
if F is None:
F = self.F
if Q is None:
Q = self.Q
elif isscalar(Q): # 如果Q是标量
Q = eye(self.dim_x) * Q
# x = Fx + Bu
if B is not None and u is not None:
self.x = dot(F, self.x) + dot(B, u)
else:
self.x = dot(F, self.x)
# P = FPF' + Q P变为状态预测向量的协方差矩阵
self.P = self.alpha_ * dot(dot(F, self.P), F.T) + Q
# save prior
self.x_prior = self.x.copy()
self.P_prior = self.P.copy()
def update(self, z, R=None, H=None):
"""
Add a new measurement (z) to the Kalman filter.
If z is None, nothing is computed. However, x_post and P_post are
updated with the prior (x_prior, P_prior), and self.z is set to None.
Parameters
----------
z : (dim_z, 1): array_like 当前历元观测向量
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
If you pass in a value of H, z must be a column vector the
of the correct size.
R : np.array, scalar, or None 测量噪声矩阵
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
H : np.array, or None 设计矩阵
Optionally provide H to override the measurement function for this
one call, otherwise self.H will be used.
"""
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
if z is None:
self.z = np.array([[None] * self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
self.y = zeros((self.dim_z, 1))
return
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
if H is None:
# z = reshape_z(z, self.dim_z, self.x.ndim)
H = self.H
# y = z - Hx 新息向量y
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# common subexpression for speed
PHT = dot(self.P, H.T)
# S = HPH' + R
# project system uncertainty into measurement space
# S 新息向量y的协方差矩阵 R观测向量噪声的协方差矩阵
self.S = dot(H, PHT) + R
self.SI = self.inv(self.S)
# K = PH'inv(S)
# map system uncertainty into kalman gain
# K 增益矩阵
self.K = dot(PHT, self.SI)
# x = x + Ky
# 当前历元的状态向量估值
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
# 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 = self._I - dot(self.K, H)
self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
文章浏览阅读52次。box-flex属性(和谐版)#father { display: box; }#first_boy { box-flex: 2; }#second_boy{ box-flex: 1; }#three_boy { box-flex: 1; }CSS box-flex属性(不和谐版)#first_boy { box-flex: 2; }#second_boy {...
文章浏览阅读492次。介绍: 在了解NSJSONSerialization之前我们需要知道JSON这个东西,JSON是什么呢!是一种轻量级的数据交换格式,更可以 理解为后台服务器传来的奇怪数据,然而NSJSONSerialization就是可以解开这个奇怪数据的方法,其实解开的方法有 很多,如:TouchJSON、SBJSON、JSONKit等等,但是NSJSONSerialization是苹果自家开发,所以性_ios nsjsonserialization da
文章浏览阅读357次。Bob,Alice和数字证书网络安全中最知名的人物大概就是Bob和Alice了,因为很多安全原理阐述中都用这两个虚拟人物来进行实例说明。我们来看看Bob是怎么从CA中心获得一个数字证书的:1、Bob首先创建他自己的密钥对(key pair),包含公钥和私钥;2、Bob通过网络把他的公钥送到CA中心,公钥中包含了Bob的个人鉴别信息(他的名字、地址、所用设备的序列号等等)_数字证书 bob alice
文章浏览阅读119次。基于JAVA便行顺风车出行系统计算机毕业设计源码+系统+lw文档+部署。springboot基于SpringBoot的商城后台管理系统。jsp基于JSP的天津城建大学计算机学院校友录管理系统。ssm基于VUE.js的保护环境的App的开发与实现。ssm基于Vue的潍坊学院宿舍管理系统的设计与实现。JSP宠物食品店系统的设计与实现sqlserver。ssm基于HTML的“守护萌宠”网站的设计与实现。ssm基于SSM的线上家庭医生系统的设计与实现。ssm基于B_S景区票务管理系统设计与实现。_顺风车辅助 csdn
文章浏览阅读135次。直接传 R.color.blackgetResources().getColor(R.color.black) 和 Color.BLACK 都是错的 会报错_immersionbar 使用自定义颜色报错
文章浏览阅读3.4k次。1、窗口定为模态的,从根本上解决问题,但是无法操作另外窗口。若不关掉此窗口,则无法操作。 使用非模态窗口,可以解决此问题,但是非模态窗口可以重复出现窗口。2、解决重复出现窗口问题: 先创建一个空指针,在创建窗体的时候判断该指针是否为NULL,如果为NULL,表示窗体未创建,则创建窗体;如果不为NULL,则表示已经创建窗体,则不再创建新窗体。用null作为判断的条件,会默认返回false// class A;// class B;void A::openB() { ..._qt 如何避免打开多个相同窗口
文章浏览阅读572次。从2016年02月21开始,逐步学习LaTeX在科技文献中的使用,该文章作为学习记录笔记。% start.tex\documentclass[utf8]{article}\begin{document}This is the start!\end{document}_texstudio中大括号怎么输入
文章浏览阅读4.5k次。<array name="country_code_list"> <item>阿富汗 *+93</item> <item>阿尔巴尼亚 *+355</item> <item>阿尔及利亚 *+213</item> <_flutter 获取第三方的国际区号列表
文章浏览阅读264次。一、Spring SpringMVC SpringBoot SpringCloud概念、关系及区别Spring主要是基于IOC反转Beans管理Bean类,主要依存于SSH框架(Struts+Spring+Hibernate)这个MVC框架,所以定位很明确,Struts主要负责表示层的显示,Spring利用它的IOC和AOP来处理控制业务(负责对数据库的操作),Hibernate主要作用是数据的持久化到数据库。SpringMVC是基于Spring的一个MVC框架,用以替代初期的SSH框架;(spring_spring,springmvc,springcloud和springboot分别指什么有什么关联
文章浏览阅读2.8k次。在学习克隆游戏对象这一节时,例子是创建一个带刚体组件的预制体,然后用脚本复制它,结果运行后发现物体向上飞了。后来发现原来是主摄像机不小心也添加了刚体组件,而场景中原本有一个不带刚体的cube1,新复制出来的带刚体的cube2和cube1碰撞了一下,斜飞了出去,而摄像机垂直掉落,导致看起来好像是两个cube一个向上飞,一个向斜上飞了。其实是一个没动,一个向斜下飞。_unity物体向上飞
文章浏览阅读391次。一、抽象类:抽象类是特殊的类,只是不能被实例化;除此以外,具有类的其他特性;重要的是抽象类可以包括抽象方法,这是普通类所不能的。抽象方法只能声明于抽象类中,且不包含任何实现,派生类必须覆盖它们。另外,抽象类可以派生自一个抽象类,可以覆盖基类的抽象方法也可以不覆盖,如果不覆盖,则其派生类必须覆盖它们。二、接口:接口是引用类型的,类似于类,和抽象类的相似之处有三点:1、不能实例化;_1.抽象类与接口的区别?
文章浏览阅读4.1k次,点赞2次,收藏7次。本地开发起了一个微服务,优先访问本地服务。需要在本地启动一个网关应用,在网关配置文件配置如下:blade: #多团队协作服务配置 ribbon: rule: #开启配置 enabled: true #负载均衡优先调用的ip段 prior-ip-pattern: - 192.168.8.6 - 127.0.0.1有限访问本地的ip地址前端vue项目配置:配置前端服务访问本地网关.._prior-ip-pattern