Kalman Filter for Localization
Process Model: If the robot pose (position and attitude) at time k is represented by the state vector qk = [xk,yk,θk]T, then the dynamics of the wheeled robot used in this experiment are well-modeled by the following set of non-linear equations:
where νk is a noise vector. Here, ΔDk is the distance traveled by the point at the center of the robot's front axle, obtained by averaging the distances measured by the left and right wheel encoders. The incremental orientation change Δθk is obtained by the onboard gyro. These dead reckoning measurements constitute the control input vector uk = [ΔDk,Δθk]T.
The system matric A(k) is represented by the Jacobian:
The input gain matrix B(k) is constructed simply:
Measurement Model: The range at time k+1 from a beacon located at (xb,yb) to the robot with state vector qk+1 can be expressed as:
The Jacobian H(k+1) gives us the measurement matrix:
Process Errors: By assuming that the process errors are uncorrelated, they can modeled as:
Here, Kss is a coefficient representing the odometric drift along the path traveled with respect to an incremental distance traveled, Ksθ represent the heading drift with respect to an incremental distance traveled, and Kθθ represents the heading drift with respect to an incremental change in heading. These coefficients were experimentally tuned to produce optimal results.
Measurement Gating: A measurement validation gate is implemeted to augment the Kalman filter's weighting of the range measurements; this is meant to disallow the degradation of the robot's state estimate by an occasional erroneous measurement. Intuitively, the greater our uncertainty about the current state estimate, the more likely we will be to incorporate an apparently erroneous measurement; conversely. if we are confident about the current state estimate then the gate should uphold a higher criteria for incoming measurements, filtering our those that are suspect. Now we compute the normalized innovation squared,
Time Propagation: When a new control input vector u(k)=[ΔDk,Δθk]T is received, the robot's state is updated according to the process model equation. Using the standard equations of Kalman filtering, the covariance matrix maintaining our uncertainty about the current state is propagated in time:
So, the state maintained during the time propagation step indicates the pose of the robot at the robot reference point.
Measurement Update: When a measurement is obtained, the update step is broken up as follows:
- Using the current state estimate, determine the location of the antenna which received the current measurement (i.e., shift the robot reference point's coordinates to get the coordinates of the current antenna, (xa,ya)).
- Project the current measurement onto the xy plan.
- Using (xa,ya) and the known beacon location (xa,ya), compute Hk.
- Look up the variance Rk and the mean yk associated with the current measurement from its stored PDF.
- Using the measurement model, compute the expected range rk to the beacon. Let ν(k) = yk - rk be the innovation.
- Compute Sk = HkP-kHTk + Rk
- Compute the Kalman gain Kk = P-kHTkS-1k.
- Compute the normalized innovation squared and test the measurement against the chi-square gating as described above.
- If the measurement passes the gating test, update the state by letting , and update the covariance matrix by letting P+k = P-k - KkSkKTk.
- Now, using this updated estimate of the pose at the antenna which reported the current measurement, shift back in x and y to get the updated pose estimate at the robot reference point.
Kalman Filter for SLAM
Process Model: Extending the formulation given for the localization model is simply a matter of expanding the state vector to contain the beacon locations. So,
where n is the number of RF beacons. Now the system matrix A(k) is represented by the Jacobian:
The input gain matrix B(k) is contructed similarly:
Measurement Model: The range at time k from a beacon b, located at (xb, yb), to the robot with state vector qk can be expressed as:
The Jacobian H(k) gives us the measurement matrix:
Only the terms in H(k) directly related to the current range measurement (i.e., the partials with respect to the robot pose and the coordinates of the tag giving the current measurement) are non-zero.
To complete the new formulation, Q is expanded to the correct dimensionality (i.e., 2n+2 square), with the extra elements filled in as zeros. Γ remains the same, and P is expanded to the same dimensionality as Q.