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}$
  • 相关阅读:
    python3 解决tcp黏包方法一
    python3 udp不黏包但丢数据
    python3 类的单例模式
    python3 day07 练习题
    python3 tcp黏包情况二
    python3 tcp黏包情况一
    python3 subprocess模块
    python3 UDP协议下的socket
    python3 TCP协议下的socket
    python3 socket网络通信的整个流程
  • 原文地址：https://www.cnblogs.com/yrm1160029237/p/10160516.html
Copyright © 2011-2022 走看看