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

Fix predict covariance (#973)

Browse Source 
* Fix predict covariance

* Remove trailing whitespace and unused return parameters
This commit is contained in:
Yi-Chen Zhang
2024-01-27 23:59:02 -05:00
committed by GitHub
parent 73d1189e82
commit 0874d5023c
2 changed files with 91 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -29,10 +29,9 @@ show_animation = True
def ekf_slam(xEst, PEst, u, z):
# Predict
S = STATE_SIZE
G, Fx = jacob_motion(xEst[0:S], u)
xEst[0:S] = motion_model(xEst[0:S], u)
PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
G, Fx = jacob_motion(xEst, u)
xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
initP = np.eye(2)
# Update
@@ -120,7 +119,7 @@ def jacob_motion(x, u):
[0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=float)
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
G = np.eye(len(x)) + Fx.T @ jF @ Fx
return G, Fx,

181
docs/modules/slam/ekf_slam/ekf_slam_main.rst
View File

@@ -63,27 +63,27 @@ Take care to note the difference between :math:`X` (state) and :math:`x`
original author: Atsushi Sakai (@Atsushi_twi)
notebook author: Andrew Tu (drewtu2)
"""
import math
import numpy as np
%matplotlib notebook
import matplotlib.pyplot as plt
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance
# Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 # Sensor Noise
Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM state size [x,y]
show_animation = True
Algorithm Walk through
@@ -97,23 +97,22 @@ the estimated state and measurements
def ekf_slam(xEst, PEst, u, z):
"""
Performs an iteration of EKF SLAM from the available information.
Performs an iteration of EKF SLAM from the available information.
:param xEst: the belief in last position
:param PEst: the uncertainty in last position
:param u: the control function applied to the last position
:param u: the control function applied to the last position
:param z: measurements at this step
:returns: the next estimated position and associated covariance
"""
S = STATE_SIZE
# Predict
xEst, PEst, G, Fx = predict(xEst, PEst, u)
xEst, PEst = predict(xEst, PEst, u)
initP = np.eye(2)
# Update
xEst, PEst = update(xEst, PEst, u, z, initP)
xEst, PEst = update(xEst, PEst, z, initP)
return xEst, PEst
@@ -170,26 +169,25 @@ the landmarks.
def predict(xEst, PEst, u):
"""
Performs the prediction step of EKF SLAM
:param xEst: nx1 state vector
:param PEst: nxn covariance matrix
:param u: 2x1 control vector
:returns: predicted state vector, predicted covariance, jacobian of control vector, transition fx
"""
S = STATE_SIZE
G, Fx = jacob_motion(xEst[0:S], u)
xEst[0:S] = motion_model(xEst[0:S], u)
G, Fx = jacob_motion(xEst, u)
xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u)
# Fx is an an identity matrix of size (STATE_SIZE)
# sigma = G*sigma*G.T + Noise
PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
return xEst, PEst, G, Fx
PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx
return xEst, PEst
.. code:: ipython3
def motion_model(x, u):
"""
Computes the motion model based on current state and input function.
:param x: 3x1 pose estimation
:param u: 2x1 control input [v; w]
:returns: the resulting state after the control function is applied
@@ -197,11 +195,11 @@ the landmarks.
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
x = (F @ x) + (B @ u)
return x
@@ -261,22 +259,21 @@ the changing uncertainty.
.. code:: ipython3
def update(xEst, PEst, u, z, initP):
def update(xEst, PEst, z, initP):
"""
Performs the update step of EKF SLAM
:param xEst: nx1 the predicted pose of the system and the pose of the landmarks
:param PEst: nxn the predicted covariance
:param u: 2x1 the control function
:param z: the measurements read at new position
:param initP: 2x2 an identity matrix acting as the initial covariance
:returns: the updated state and covariance for the system
"""
for iz in range(len(z[:, 0])): # for each observation
minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark
nLM = calc_n_LM(xEst) # number of landmarks we currently know about
if minid == nLM: # Landmark is a NEW landmark
print("New LM")
# Extend state and covariance matrix
@@ -285,14 +282,14 @@ the changing uncertainty.
np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
xEst = xAug
PEst = PAug
lm = get_LM_Pos_from_state(xEst, minid)
y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain
xEst = xEst + (K @ y)
PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst
xEst[2] = pi_2_pi(xEst[2])
return xEst, PEst
@@ -302,7 +299,7 @@ the changing uncertainty.
def calc_innovation(lm, xEst, PEst, z, LMid):
"""
Calculates the innovation based on expected position and landmark position
:param lm: landmark position
:param xEst: estimated position/state
:param PEst: estimated covariance
@@ -315,19 +312,19 @@ the changing uncertainty.
zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
# zp is the expected measurement based on xEst and the expected landmark position
y = (z - zp).T # y = innovation
y[1] = pi_2_pi(y[1])
H = jacobH(q, delta, xEst, LMid + 1)
S = H @ PEst @ H.T + Cx[0:2, 0:2]
return y, S, H
def jacobH(q, delta, x, i):
"""
Calculates the jacobian of the measurement function
:param q: the range from the system pose to the landmark
:param delta: the difference between a landmark position and the estimated system position
:param x: the state, including the estimated system position
@@ -337,17 +334,17 @@ the changing uncertainty.
sq = math.sqrt(q)
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
[delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
G = G / q
nLM = calc_n_LM(x)
F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))
F = np.vstack((F1, F2))
H = G @ F
return H
Observation Step
@@ -370,17 +367,17 @@ reckoning and control functions are passed along here as well.
:param xd: the current noisy estimate of the system
:param u: the current control input
:param RFID: the true position of the landmarks
:returns: Computes the true position, observations, dead reckoning (noisy) position,
:returns: Computes the true position, observations, dead reckoning (noisy) position,
and noisy control function
"""
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])): # Test all beacons, only add the ones we can see (within MAX_RANGE)
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
@@ -390,12 +387,12 @@ reckoning and control functions are passed along here as well.
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.array([dn, anglen, i])
z = np.vstack((z, zi))
# add noise to input
ud = np.array([[
u[0, 0] + np.random.randn() * Rsim[0, 0],
u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
@@ -409,32 +406,30 @@ reckoning and control functions are passed along here as well.
"""
n = int((len(x) - STATE_SIZE) / LM_SIZE)
return n
def jacob_motion(x, u):
"""
Calculates the jacobian of motion model.
Calculates the jacobian of motion model.
:param x: The state, including the estimated position of the system
:param u: The control function
:returns: G: Jacobian
Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix
"""
# [eye(3) [0 x y; 0 x y; 0 x y]]
Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
jF = np.array([[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]],dtype=object)
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
if calc_n_LM(x) > 0:
print(Fx.shape)
return G, Fx,
.. code:: ipython3
@@ -442,68 +437,68 @@ reckoning and control functions are passed along here as well.
def calc_LM_Pos(x, z):
"""
Calculates the pose in the world coordinate frame of a landmark at the given measurement.
:param x: [x; y; theta]
:param z: [range; bearing]
:returns: [x; y] for given measurement
"""
zp = np.zeros((2, 1))
zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
#zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
#zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
return zp
def get_LM_Pos_from_state(x, ind):
"""
Returns the position of a given landmark
:param x: The state containing all landmark positions
:param ind: landmark id
:returns: The position of the landmark
"""
lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
return lm
def search_correspond_LM_ID(xAug, PAug, zi):
"""
Landmark association with Mahalanobis distance.
If this landmark is at least M_DIST_TH units away from all known landmarks,
If this landmark is at least M_DIST_TH units away from all known landmarks,
it is a NEW landmark.
:param xAug: The estimated state
:param PAug: The estimated covariance
:param zi: the read measurements of specific landmark
:returns: landmark id
"""
nLM = calc_n_LM(xAug)
mdist = []
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T @ np.linalg.inv(S) @ y)
mdist.append(M_DIST_TH) # new landmark
minid = mdist.index(min(mdist))
return minid
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
return u
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
@@ -511,53 +506,53 @@ reckoning and control functions are passed along here as well.
def main():
print(" start!!")
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, -2.0],
[15.0, 10.0],
[3.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.zeros((STATE_SIZE, 1))
xTrue = np.zeros((STATE_SIZE, 1))
PEst = np.eye(STATE_SIZE)
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
x_state = xEst[0:STATE_SIZE]
# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
if show_animation: # pragma: no cover
plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(xEst[0], xEst[1], ".r")
# plot landmark
for i in range(calc_n_LM(xEst)):
plt.plot(xEst[STATE_SIZE + i * 2],
xEst[STATE_SIZE + i * 2 + 1], "xg")
plt.plot(hxTrue[0, :],
hxTrue[1, :], "-b")
plt.plot(hxDR[0, :],
@@ -587,5 +582,3 @@ References:
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- `PROBABILISTIC ROBOTICS <http://www.probabilistic-robotics.org/>`_