github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-12 23:58:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2019-03-10 08:35:03 +09:00
parent f419f6ac27
commit 94e03e1e53
1 changed files with 1 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -3,8 +3,8 @@ Extended Kalman Filter SLAM example
author: Atsushi Sakai (@Atsushi_twi)
"""
import numpy as np
import math
import numpy as np
import matplotlib.pyplot as plt
@@ -169,7 +169,6 @@ def search_correspond_LM_ID(xAug, PAug, zi):
def calc_innovation(lm, xEst, PEst, z, LMid):
delta = lm - xEst[0:2]
q = (delta.T @ delta)[0, 0]
#zangle = math.atan2(delta[1], delta[0]) - xEst[2]
zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
y = (z - zp).T