extended_kalman_filter Namespace Reference

Functions
list[float]	get_u ()
list[float]	motion_model (list[float] x, list[float] u)
list[float]	jacob_f (list[float] x, list[float] u)
list[float]	observation_model (list[float] x)
ObservationOutput	observation (list[float] xTrue, list[float] xd, list[float] u)
	jacob_h ()
EKFEstimationOutput	ekf_estimation (xEst, PEst, z, u)
	main ()

Variables
str	BROKER = "localhost"
int	PORT = 1883
str	GPS_TOPIC = "GPS_TOPIC"
str	INPUT_TOPIC = "U_TOPIC"
str	STATE_TOPIC = "STATE_TOPIC"
bool	SHOW_ANIMATION = False
int	INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
int	GPS_NOISE = np.diag([0.5, 0.5]) ** 2
float	DT = 0.1
float	SIM_TIME = 50.0
	F
	H
int	Q
int	R = np.diag([1.0, 1.0]) ** 2
	ObservationOutput
	EKFEstimationOutput

Detailed Description

Extended kalman filter (EKF) localization sample

author: Atsushi Sakai (@Atsushi_twi)

This file is edited from
https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py

Function Documentation

ekf_estimation()

EKFEstimationOutput extended_kalman_filter.ekf_estimation	(		xEst,
			PEst,
			z,
			u
	)

get_u()

list[float] extended_kalman_filter.get_u

(

)

jacob_f()

list[float] extended_kalman_filter.jacob_f	(	list[float]	x,
		list[float]	u
	)

Jacobian of Motion Model

motion model
x_{t+1} = x_t+v*dt*cos(yaw)
y_{t+1} = y_t+v*dt*sin(yaw)
yaw_{t+1} = yaw_t+omega*dt
v_{t+1} = v{t}
so
dx/dyaw = -v*dt*sin(yaw)
dx/dv = dt*cos(yaw)
dy/dyaw = v*dt*cos(yaw)
dy/dv = dt*sin(yaw)

jacob_h()

extended_kalman_filter.jacob_h

(

)

main()

extended_kalman_filter.main

(

)

motion_model()

list[float] extended_kalman_filter.motion_model	(	list[float]	x,
		list[float]	u
	)

observation()

ObservationOutput extended_kalman_filter.observation	(	list[float]	xTrue,
		list[float]	xd,
		list[float]	u
	)

observation_model()

list[float] extended_kalman_filter.observation_model

(

list[float]

x

)

Variable Documentation

BROKER

str extended_kalman_filter.BROKER = "localhost"

DT

float extended_kalman_filter.DT = 0.1

EKFEstimationOutput

extended_kalman_filter.EKFEstimationOutput

Initial value:

=  namedtuple(typename='EKFEstimationOutput',
                                 field_names=['x_est', 'p_est'])

F

extended_kalman_filter.F

Initial value:

=  np.array([[1.0, 0, 0, 0],
              [0, 1.0, 0, 0],
              [0, 0, 1.0, 0],
              [0, 0, 0, 0]])

GPS_NOISE

int extended_kalman_filter.GPS_NOISE = np.diag([0.5, 0.5]) ** 2

GPS_TOPIC

str extended_kalman_filter.GPS_TOPIC = "GPS_TOPIC"

H

extended_kalman_filter.H

Initial value:

=  np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0]
])

INPUT_NOISE

int extended_kalman_filter.INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2

INPUT_TOPIC

str extended_kalman_filter.INPUT_TOPIC = "U_TOPIC"

ObservationOutput

extended_kalman_filter.ObservationOutput

Initial value:

=  namedtuple(typename='ObservationOutput',
                               field_names=['x_true', 'gps_sensor', 'xd', 'ud'])

PORT

int extended_kalman_filter.PORT = 1883

Q

int extended_kalman_filter.Q

Initial value:

=  np.diag([
    0.1,  # variance of location on x-axis
    0.1,  # variance of location on y-axis
    np.deg2rad(1.0),  # variance of yaw angle
    1.0  # variance of velocity
]) ** 2

R

int extended_kalman_filter.R = np.diag([1.0, 1.0]) ** 2

SHOW_ANIMATION

bool extended_kalman_filter.SHOW_ANIMATION = False

SIM_TIME

float extended_kalman_filter.SIM_TIME = 50.0

STATE_TOPIC

str extended_kalman_filter.STATE_TOPIC = "STATE_TOPIC"