From 320f0c4d896869ceea0102438e8271cbb42e4a43 Mon Sep 17 00:00:00 2001 From: Alec Li Date: Thu, 25 Apr 2019 18:57:24 -0400 Subject: [PATCH] added partial documentation to ekf methods --- SLAM/EKFSLAM/ekf_slam.ipynb | 75 ++++++++++++++++++++++++++----------- 1 file changed, 54 insertions(+), 21 deletions(-) diff --git a/SLAM/EKFSLAM/ekf_slam.ipynb b/SLAM/EKFSLAM/ekf_slam.ipynb index d5094441..843366e4 100644 --- a/SLAM/EKFSLAM/ekf_slam.ipynb +++ b/SLAM/EKFSLAM/ekf_slam.ipynb @@ -173,9 +173,9 @@ " \n", " :param xEst: the belief in last position\n", " :param PEst: the uncertainty in last position\n", - " :param u: the control function applied to the last position \n", - " :param z: measurements at this step\n", - " :returns: the next estimated position and associated covariance\n", + " :param u: the control function applied to the last position \n", + " :param z: measurements at this step\n", + " :returns: the next estimated position and associated covariance\n", " \"\"\"\n", " S = STATE_SIZE\n", "\n", @@ -301,8 +301,8 @@ " \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", + " :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", @@ -404,11 +404,12 @@ " \"\"\"\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", + " :param xEst: nx1 the predicted pose of the system and the pose of the landmarks\n", + " :param PEst: nxn the predicted covariance\n", + " :param u: 2x1 the control function \n", + " :param z: the measurements read at new position\n", + " :param initP: 2x2 an identity matrix acting as the initial covariance\n", + " :returns: the updated state and covariance for the system\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", @@ -427,7 +428,7 @@ " lm = get_LM_Pos_from_state(xEst, minid)\n", " y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)\n", "\n", - " K = (PEst @ H.T) @ np.linalg.inv(S) # Calcualte Kalman Gain\n", + " K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain\n", " xEst = xEst + (K @ y)\n", " PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst\n", " \n", @@ -442,6 +443,16 @@ "outputs": [], "source": [ "def calc_innovation(lm, xEst, PEst, z, LMid):\n", + " \"\"\"\n", + " Calculates the innovation based on expected position and landmark position\n", + " \n", + " :param lm: landmark position\n", + " :param xEst: estimated position/state\n", + " :param PEst: estimated covariance\n", + " :param z: read measurements\n", + " :param LMid: landmark id\n", + " :returns: returns the innovation y, xxxxxxxxxxxx, and the jacobian H\n", + " \"\"\"\n", " delta = lm - xEst[0:2]\n", " q = (delta.T @ delta)[0, 0]\n", " zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]\n", @@ -457,6 +468,15 @@ " return y, S, H\n", "\n", "def jacobH(q, delta, x, i):\n", + " \"\"\"\n", + " Calculates the jacobian of the measurement function\n", + " \n", + " :param q: \n", + " :param delta: the difference between a landmark position and the estimated system position\n", + " :param x: the state, including the estimated system position\n", + " :param i: landmark id + 1\n", + " :returns: the jacobian H\n", + " \"\"\"\n", " sq = math.sqrt(q)\n", " G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],\n", " [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])\n", @@ -497,12 +517,12 @@ "def observation(xTrue, xd, u, RFID):\n", " \"\"\"\n", " :param xTrue: the true pose of the system\n", - " :param xd: the current noisy estimate of the system\n", - " :param u: the current control input\n", - " :param RFID: the true position of the landmarks\n", + " :param xd: the current noisy estimate of the system\n", + " :param u: the current control input\n", + " :param RFID: the true position of the landmarks\n", " \n", - " :returns: Computes the true position, observations, dead reckoning (noisy) position, \n", - " and noisy control function\n", + " :returns: Computes the true position, observations, dead reckoning (noisy) position, \n", + " and noisy control function\n", " \"\"\"\n", " xTrue = motion_model(xTrue, u)\n", "\n", @@ -539,6 +559,8 @@ "def calc_n_LM(x):\n", " \"\"\"\n", " Calculates the number of landmarks currently tracked in the state\n", + " :param x: the state\n", + " :returns: the number of landmarks n\n", " \"\"\"\n", " n = int((len(x) - STATE_SIZE) / LM_SIZE)\n", " return n\n", @@ -548,8 +570,10 @@ " \"\"\"\n", " Calculates the jacobian of motion model. \n", " \n", - " :param G: Jacobian\n", - " :param Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix\n", + " :param x: The state, including the estimated position of the system\n", + " :param u: The control function\n", + " :returns: G: Jacobian\n", + " Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix\n", " \"\"\"\n", " \n", " # [eye(3) [0 x y; 0 x y; 0 x y]]\n", @@ -593,7 +617,13 @@ "\n", "\n", "def get_LM_Pos_from_state(x, ind):\n", - "\n", + " \"\"\"\n", + " Returns the position of a given landmark\n", + " \n", + " :param x: The state containing all landmark positions\n", + " :param ind: landmark id\n", + " :returns: The position of the landmark\n", + " \"\"\"\n", " lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]\n", "\n", " return lm\n", @@ -606,7 +636,10 @@ " If this landmark is at least M_DIST_TH units away from all known landmarks, \n", " it is a NEW landmark.\n", " \n", - " :returns: landmark id\n", + " :param xAug: The estimated state\n", + " :param PAug: The estimated covariance\n", + " :param zi: the read measurements of specific landmark\n", + " :returns: landmark id\n", " \"\"\"\n", "\n", " nLM = calc_n_LM(xAug)\n", @@ -1540,7 +1573,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.6.1" + "version": "3.7.2" } }, "nbformat": 4,