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

small updates to pydoc

Browse Source
This commit is contained in:
Andrew Tu
2019-04-24 19:06:00 -04:00
parent b51fc3998c
commit 04a423211c
1 changed files with 23 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
SLAM/EKFSLAM/ekf_slam.ipynb
View File

@@ -163,7 +163,7 @@
},
{
"cell_type": "code",
"execution_count": 18,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@@ -180,7 +180,7 @@
" S = STATE_SIZE\n",
"\n",
" # Predict\n",
" xEst, PEst, G, Fx = predict(xEst, PEst, u, z)\n",
" xEst, PEst, G, Fx = predict(xEst, PEst, u)\n",
" initP = np.eye(2)\n",
"\n",
" # Update\n",
@@ -291,11 +291,19 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"def predict(xEst, PEst, u, z):\n",
"def predict(xEst, PEst, u):\n",
" \"\"\"\n",
" Performs the prediction step of EKF SLAM\n",
" \n",
" :param xEst: nx1 state vector\n",
" :param PEst: nxn covariacne matrix\n",
" :param u: 2x1 control vector\n",
" :returns: predicted state vector, predicted covariance, jacobian of control vector, transition fx\n",
" \"\"\"\n",
" S = STATE_SIZE\n",
" xEst[0:S] = motion_model(xEst[0:S], u)\n",
" G, Fx = jacob_motion(xEst[0:S], u)\n",
@@ -307,7 +315,7 @@
},
{
"cell_type": "code",
"execution_count": 8,
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@@ -317,7 +325,7 @@
" \n",
" :param x: 3x3 pose estimation\n",
" :param u: 2x1 control input [v; w]\n",
" :returns: x' = Fx + Bu\n",
" :returns: the resutling state after the control function is applied\n",
" \"\"\"\n",
" F = np.array([[1.0, 0, 0],\n",
" [0, 1.0, 0],\n",
@@ -393,6 +401,15 @@
"outputs": [],
"source": [
"def update(xEst, PEst, u, z, initP):\n",
" \"\"\"\n",
" Performs the update step of EKF SLAM\n",
" \n",
" :param xEst:\n",
" :param PEst:\n",
" :param u:\n",
" :param z:\n",
" :param initP:\n",
" \"\"\"\n",
" for iz in range(len(z[:, 0])): # for each observation\n",
" minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark\n",
"\n",