mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
added partial documentation to ekf methods
This commit is contained in:
Alec Li
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user