AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 22:57:55 -05:00
Code Issues Packages Projects Releases Wiki Activity

keep implementing

Browse Source
This commit is contained in:
Atsushi Sakai
2018-03-03 09:07:47 -08:00
parent f0a654ec3f
commit ae44021092
1 changed files with 34 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -10,6 +10,8 @@ import numpy as np
import math
import matplotlib.pyplot as plt
Cx = np.diag([1.0, 1.0, math.radians(30.0)])**2
# Simulation parameter
Qsim = np.diag([0.2])**2
Rsim = np.diag([1.0, math.radians(30.0)])**2
@@ -18,6 +20,9 @@ DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
POSE_SIZE = 3 # [x,y,yaw]
LM_SIZE = 2 # [x,y]
# Particle filter parameter
NP = 100 # Number of Particle
NTh = NP / 2.0 # Number of particle for re-sampling
@@ -61,26 +66,43 @@ def observation(xTrue, xd, u, RFID):
def motion_model(x, u):
F = np.matrix([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
F = np.matrix([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
B = np.matrix([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
[0.0, DT]])
x = F * x + B * u
return x
def calc_n_LM(x):
n = int((len(x) - POSE_SIZE) / LM_SIZE)
return n
def jacob_motion(x, u):
Fx = np.hstack((np.eye(POSE_SIZE), np.zeros(
(POSE_SIZE, LM_SIZE * calc_n_LM(x)))))
jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]])
G = np.eye(POSE_SIZE) + Fx.T * jF * Fx
return G, Fx,
def ekf_slam(xEst, PEst, u, z):
# Predict
xEst = motion_model(xEst, u)
# [G,Fx]=jacobF(xEst, u);
# PEst= G'*PEst*G + Fx'*R*Fx;
G, Fx = jacob_motion(xEst, u)
PEst = G.T * PEst * G + Fx.T * Cx * Fx
return xEst, PEst
@@ -137,11 +159,11 @@ def main():
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((4, 1)))
xTrue = np.matrix(np.zeros((4, 1)))
PEst = np.eye(4)
xEst = np.matrix(np.zeros((POSE_SIZE, 1)))
xTrue = np.matrix(np.zeros((POSE_SIZE, 1)))
PEst = np.eye(POSE_SIZE)
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
xDR = np.matrix(np.zeros((POSE_SIZE, 1))) # Dead reckoning
# history
hxEst = xEst