EKF model&realization - 走看看

zoukankan html css js c++ java

EKF model&realization

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$ .
$extbf{x}_t=[x_t, y_t, heta_t, v_t]$
x, y are a 2D x-y position, $heta$ 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
$extbf{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.
$extbf{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
$extbf{x}_{t+1} = F extbf{x}_t+B extbf{u}_t$
where

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

$egin{equation*} B= egin{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

$egin{equation*} J_F= egin{bmatrix} frac{dx}{dx}& frac{dx}{dy} & frac{dx}{dphi} & frac{dx}{dv}\ frac{dy}{dx}& frac{dy}{dy} & frac{dy}{dphi} & frac{dy}{dv}\ frac{dphi}{dx}& frac{dphi}{dy} & frac{dphi}{dphi} & frac{dphi}{dv}\ frac{dv}{dx}& frac{dv}{dy} & frac{dv}{dphi} & frac{dv}{dv}\ end{bmatrix} end{equation*}$

$egin{equation*} 　= egin{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
$extbf{z}_{t} = H extbf{x}_t$
where

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

Its Jacobian matrix is

$egin{equation*} J_H= egin{bmatrix} frac{dx}{dx}& frac{dx}{dy} & frac{dx}{dphi} & frac{dx}{dv}\ frac{dy}{dx}& frac{dy}{dy} & frac{dy}{dphi} & frac{dy}{dv}\ end{bmatrix} end{equation*}$

$egin{equation*} 　= egin{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}$

查看全文

相关阅读:
c# asp.net 鼠标改变控件坐标位置,更改控件坐标,注册表保存读取,打印,查找局域网内打印机等等收集
 jquery,extjs中的extend用法小结
 JavaScript创建Map对象（转）
ExtJS与jQuery的一点细节上的对比
 jQuery 的原型关系图，整体把握jQuery
github网站介绍、并使用git命令管理github(详细描述)
1.移植3.4内核-分析内核启动过程,重新分区,烧写jffs2文件系统
 6.移植uboot-支持yaffs烧写,打补丁
 5.移植uboot-设置默认环境变量,裁剪,并分区
 4.移植uboot-使uboot支持DM9000网卡

原文地址：https://www.cnblogs.com/yrm1160029237/p/10160516.html

Copyright © 2011-2022 走看看