卡尔曼滤波— Constant Velocity Model
假设你开车进入隧道,GPS信号丢失,现在我们要确定汽车在隧道内的位置。汽车的绝对速度可以通过车轮转速计算得到,汽车朝向可以通过yaw rate sensor(A yaw-rate sensor is a gyroscopic device that measures a vehicle’s angular velocity around its vertical axis. )得到,因此可以获得X轴和Y轴速度分量Vx,Vy
首先确定状态变量,恒速度模型中取状态变量为汽车位置和速度:
根据运动学定律(The basic idea of any motion models is that a mass cannot move arbitrarily due to inertia):
由于GPS信号丢失,不能直接测量汽车位置,则观测模型为:
卡尔曼滤波步骤如下图所示:
# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt # Initial State x0
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T # Initial Uncertainty P0
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0]) dt = 0.1 # Time Step between Filter Steps # Dynamic Matrix A
A = np.matrix([[1.0, 0.0, dt, 0.0],
[0.0, 1.0, 0.0, dt],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]) # Measurement Matrix
# We directly measure the velocity vx and vy
H = np.matrix([[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]) # Measurement Noise Covariance
ra = 10.0**2
R = np.matrix([[ra, 0.0],
[0.0, ra]]) # Process Noise Covariance
# The Position of the car can be influenced by a force (e.g. wind), which leads
# to an acceleration disturbance (noise). This process noise has to be modeled
# with the process noise covariance matrix Q.
sv = 8.8
G = np.matrix([[0.5*dt**2],
[0.5*dt**2],
[dt],
[dt]])
Q = G*G.T*sv**2 I = np.eye(4) # Measurement
m = 200 # 200个测量点
vx= 20 # in X
vy= 10 # in Y
mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my)) # Preallocation for Plotting
xt = []
yt = [] # Kalman Filter
for n in range(len(measurements[0])): # Time Update (Prediction)
# ========================
# Project the state ahead
x = A*x # Project the error covariance ahead
P = A*P*A.T + Q # Measurement Update (Correction)
# ===============================
# Compute the Kalman Gain
S = H*P*H.T + R
K = (P*H.T) * np.linalg.pinv(S) # Update the estimate via z
Z = measurements[:,n].reshape(2,1)
y = Z - (H*x) # Innovation or Residual
x = x + (K*y) # Update the error covariance
P = (I - (K*H))*P # Save states for Plotting
xt.append(float(x[0]))
yt.append(float(x[1])) # State Estimate: Position (x,y)
fig = plt.figure(figsize=(16,16))
plt.scatter(xt,yt, s=20, label='State', c='k')
plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r') plt.xlabel('X')
plt.ylabel('Y')
plt.title('Position')
plt.legend(loc='best')
plt.axis('equal')
plt.show()
汽车在隧道中的估计位置如下图:
参考
Improving IMU attitude estimates with velocity data
https://zhuanlan.zhihu.com/p/25598462
卡尔曼滤波— Constant Velocity Model的更多相关文章
- 卡尔曼滤波—Simple Kalman Filter for 2D tracking with OpenCV
之前有关卡尔曼滤波的例子都比较简单,只能用于简单的理解卡尔曼滤波的基本步骤.现在让我们来看看卡尔曼滤波在实际中到底能做些什么吧.这里有一个使用卡尔曼滤波在窗口内跟踪鼠标移动的例子,原作者主页:http ...
- (转) Deep Reinforcement Learning: Pong from Pixels
Andrej Karpathy blog About Hacker's guide to Neural Networks Deep Reinforcement Learning: Pong from ...
- Mini-project # 4 - "Pong"___An Introduction to Interactive Programming in Python"RICE"
Mini-project #4 - "Pong" In this project, we will build a version of Pong, one of the firs ...
- RootMotionComputer 根运动计算机
using UnityEngine; using System.Collections; /* * -------------------------------------------------- ...
- Framework for Graphics Animation and Compositing Operations
FIELD OF THE DISCLOSURE The subject matter of the present disclosure relates to a framework for hand ...
- Tracking without bells and whistles
Tracking without bells and whistles 2019-08-07 20:46:12 Paper: https://arxiv.org/pdf/1903.05625 Code ...
- [Elementary Mechanics Using Python-02]Feather in tornado
Problem 9.17 Feather in tornado. In this project you will learn to use Newton's laws and the force m ...
- [UE4]自定义MovementComponent组件
自定义Movement组件 目的:实现自定义轨迹如抛物线,线性,定点等运动方式,作为组件控制绑定对象的运动. 基类:UMovementComponent 过程: 1.创建UCustomMovement ...
- UIScrollview使用
改变内容偏移 - (void)setContentOffset:(CGPoint)contentOffset animated:(BOOL)animated; // animate at const ...
随机推荐
- Asp.net中后台C#数组与前台Javascript数组交互
摘自:http://blog.csdn.net/a6225301/article/details/20003305 在上一篇<asp.net中javascript与后台c#交互>中实现了前 ...
- 三、Java基础---------关于继承、构造函数、静态代码块执行顺序示例讲解
在上节博客中曾提到过类的继承,这篇文章主要是介绍类的继承.构造函数以及静态代码块的执行顺序. 首先接着分析在黑马基础测试中的一个关于继承的题目,题目描述如下: 声明类Person,包含2个成员变量:n ...
- Openstack的配额共功能的使用
在一个云系统中,一个项目不能无限制的使用资源,必须对项目进行配额管理,在openstack中主要的命令是nova quota-update, 但是可能会提示的错误: DEBUG (shell:740) ...
- 图解IoC 依赖注入
- Error while trying to retrieve text for error ORA-01019 的解决办法
这个问题涉及到 64 位的oracle服务, 和32位的客户端导致的问题. 环境如下: win8.1 + 64位oracle 11.1 做服务器, 客户端由于采用32位程序,不支持64位的oracle ...
- ERR: Call to undefined function openssl_random_pseudo_bytes()
最近使用TP5/PHP7,总是出现ERR: Call to undefined function index\index\openssl_random_pseudo_bytes(),才发现是php没有 ...
- Add baidu map in your website (wordpress)
手动挡 访问应用(AK)Key http://lbsyun.baidu.com/apiconsole/key Basic Map Generator http://api.map.baidu.com/ ...
- HDU 4417:Super Mario(主席树)
http://acm.hdu.edu.cn/showproblem.php?pid=4417 题意是:给出n个数和q个询问,每个询问有一个l,r,h,问在[l,r]这个区间里面有多少个数是小于等于h的 ...
- IIS管理网站浏览
7.“/”应用程序中的服务器错误. 分析器错误 说明: 在分析向此请求提供服务所需资源时出错.请检查下列特定分析错误详细信息并适当地修改源文件.分析器错误消息: 文件“/Default.aspx.cs ...
- 在Ecshop后台打印订单页面将商品按货号排序
ECSHOP后台管理里的“打印订单" 页面里的商品排序有点乱,现在想改成按序号来排序,修改方法如下 下面是在2.7.2基础上做的修改 打开 admin/order.php 文件 找到(大约 ...