github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Adding NDT mapping script and doc (#867)

Browse Source 
* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc
This commit is contained in:
Atsushi Sakai
2023-07-06 23:12:43 +09:00
committed by GitHub
parent 49dd1a93f3
commit 0fc769421f
12 changed files with 371 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -7,13 +7,14 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import math
import matplotlib.pyplot as plt
import numpy as np
from utils.angle import rot_mat_2d
from utils.plot import plot_covariance_ellipse
# Covariance for EKF simulation
Q = np.diag([
@@ -135,29 +136,6 @@ def ekf_estimation(xEst, PEst, z, u):
return xEst, PEst
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)
if eigval[0] >= eigval[1]:
bigind = 0
smallind = 1
else:
bigind = 1
smallind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
a = math.sqrt(eigval[bigind])
b = math.sqrt(eigval[smallind])
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
fx = rot_mat_2d(angle) @ (np.array([x, y]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
def main():
print(__file__ + " start!!")
@@ -202,7 +180,7 @@ def main():
hxDR[1, :].flatten(), "-k")
plt.plot(hxEst[0, :].flatten(),
hxEst[1, :].flatten(), "-r")
plot_covariance_ellipse(xEst, PEst)
plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)