EKF model&realization

寒江小筑 2024-11-01 07:48:53 原文

REF： https://pythonrobotics.readthedocs.io/en/latest/modules/localization.html#unscented-kalman-filter-localization

Filter design

In this simulation, the robot has a state vector includes 4 states at time $t$ .

$\textbf{x}_t=[x_t, y_t, \theta_t, v_t]$

x, y are a 2D x-y position, $\theta$ is orientation, and v is velocity.

In the code, "xEst" means the state vector. code

And, $P_t$ is covariace matrix of the state,

$Q$ is covariance matrix of process noise,

$R$ is covariance matrix of observation noise at time $t$

　

The robot has a speed sensor and a gyro sensor.

So, the input vecor can be used as each time step

$\textbf{u}_t=[v_t, \omega_t]$

Also, the robot has a GNSS sensor, it means that the robot can observe x-y position at each time.

$\textbf{z}_t=[x_t,y_t]$

The input and observation vector includes sensor noise.

In the code, "observation" function generates the input and observation vector with noise code

Motion Model

The robot model is

$\dot{x} = vcos(\phi)$$$$ \dot{y} = vsin((\phi)$$$$ \dot{\phi} = \omega$

So, the motion model is

$\textbf{x}_{t+1} = F\textbf{x}_t+B\textbf{u}_t$

where

$\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}$

$\begin{equation*} B= \begin{bmatrix} cos(\phi)dt & 0\\ sin(\phi)dt & 0\\ 0 & dt\\ 1 & 0\\ \end{bmatrix} \end{equation*}$

$dt$ is a time interval.

This is implemented at code

Its Jacobian matrix is

$\begin{equation*} J_F= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \frac{d\phi}{dx}& \frac{d\phi}{dy} & \frac{d\phi}{d\phi} & \frac{d\phi}{dv}\\ \frac{dv}{dx}& \frac{dv}{dy} & \frac{dv}{d\phi} & \frac{dv}{dv}\\ \end{bmatrix} \end{equation*}$

$\begin{equation*} = \begin{bmatrix} 1& 0 & -v sin(\phi)dt & cos(\phi)dt\\ 0 & 1 & v cos(\phi)dt & sin(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}$

Observation Model

The robot can get x-y position infomation from GPS.

So GPS Observation model is

$\textbf{z}_{t} = H\textbf{x}_t$

where

$\begin{equation*} B= \begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0& 0\\ \end{bmatrix} \end{equation*}$

Its Jacobian matrix is

$\begin{equation*} J_H= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \end{bmatrix} \end{equation*}$

$\begin{equation*} = \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}$

Extented Kalman Filter

Localization process using Extendted Kalman Filter:EKF is

=== Predict ===

$x_{Pred} = Fx_t+Bu_t$

$P_{Pred} = J_FP_t J_F^T + Q$

=== Update ===

$z_{Pred} = Hx_{Pred}$

$y = z - z_{Pred}$

$S = J_H P_{Pred}.J_H^T + R$

$K = P_{Pred}.J_H^T S^{-1}$

$x_{t+1} = x_{Pred} + Ky$

$P_{t+1} = ( I - K J_H) P_{Pred}$

EKF model&realization的更多相关文章

Awesome Delphi
Awesome Delphi A curated list of awesome Delphi frameworks, libraries, resources, and shiny things. ...
Spring Boot笔记一
Spring Boot 入门 Spring Boot 简介 > 简化Spring应用开发的一个框架:> 整个Spring技术栈的一个大整合:> J2EE开发的一站式解决方案: 微服务 ...
EKF的理解
若已知参考点(landmarks)的坐标,则状态向量中不必含有xL, 从而实现的仅为机器人在已知环境中的定位,求解大大减少(状态向量维度仅为运动状态).若想实现完整SLAM,必须将xL加入状态向量中. ...
学习笔记TF053:循环神经网络，TensorFlow Model Zoo，强化学习，深度森林，深度学习艺术
循环神经网络.https://github.com/aymericdamien/TensorFlow-Examples/blob/master/examples/3_NeuralNetworks/re ...
Multi-core compute cache coherency with a release consistency memory ordering model
A method includes storing, with a first programmable processor, shared variable data to cache lines ...
【疯狂造轮子-iOS】JSON转Model系列之二
[疯狂造轮子-iOS]JSON转Model系列之二本文转载请注明出处 —— polobymulberry-博客园 1. 前言上一篇<[疯狂造轮子-iOS]JSON转Model系列之一> ...
【疯狂造轮子-iOS】JSON转Model系列之一
[疯狂造轮子-iOS]JSON转Model系列之一本文转载请注明出处 —— polobymulberry-博客园 1. 前言之前一直看别人的源码,虽然对自己提升比较大,但毕竟不是自己写的,很容易遗 ...
详解树莓派Model B+控制蜂鸣器演奏乐曲
步进电机以及无源蜂鸣器这些都需要脉冲信号才能够驱动,这里将用GPIO的PWM接口驱动无源蜂鸣器弹奏乐曲,本文基于树莓派Mode B+,其他版本树莓派实现时需参照相关资料进行修改! 1 预备知识 1.1 ...
【AutoMapper官方文档】DTO与Domin Model相互转换（上）
写在前面 AutoMapper目录: [AutoMapper官方文档]DTO与Domin Model相互转换(上) [AutoMapper官方文档]DTO与Domin Model相互转换(中) [Au ...

随机推荐

复制的web工程为什么不能部署到tomcat
An existing resource has been found at location D:\apache-tomcat-6.0.18\apache-tomcat-6.0.18\webapps ...
flask的request的用法
其中在头部取值是这样的,request.headers,得到的是一个字典参考链接:http://blog.csdn.net/yannanxiu/article/details/53116652
jquery选择器之获取父级元素，子元素，同级元素
<!DOCTYPE html> <html> <head> <meta charset="utf-8"> <link href ...
ArcGIS破解配置及oracle文件配置
1.破解配置 2.oracle文件配置
centos6.x 配置bond
centos6.x 配置bond centos6.x 配置bond1 物理网卡配置2 bond0网卡配置3 查看bond0网卡状态摘要: centos6.x下使用双网卡配置bond0, centos ...
The type org.springframework.context.ConfigurableApplicationContext cannot be resolved问题解决
在搭建maven项目的时候,有时候会报这样的问题. The type org.springframework.context.ConfigurableApplicationContext cannot ...
Could not find class 'org.ksoap2.serialization.SoapObject
Could not find class 'org.ksoap2.serialization.SoapObject工程编译没问题,一在模拟器运行就报错! 这是由于ADT版本过高引发的问题,解决办法: ...
DataSnap侦听端口动态设置问题
使用DataSnap做服务器,端口设置为可配置,然后在程序中动态指定: procedure TscUPower.DataModuleCreate(Sender: TObject); begin dst ...
redis list结构
一个功能肯定有其应用场景: PUSH和POP操作,其实是队列的基本操作.Redis的list就是一个极其强大的队列系统.我们在哪些地方会用到队列呢?下面,我们说两个例子: a,评论系统逛过微博的筒子 ...
Jenkins+SVN+maven+Tomcat构建自动化集成任务
Jenkins安装方法详解:https://www.cnblogs.com/lizhe860/p/9901257.html 一.安装maven插件 1.依次进入系统管理→插件管理→可选插件, 找到Ma ...