Kalman Filter Localization

Kalman Filter for Localization

Process Model: If the robot pose (position and attitude) at time k is represented by the state vector q_k = [x_k,y_k,θ_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, ΔD_k 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 u_k = [ΔD_k,Δθ_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 (x_b,y_b) to the robot with state vector q_k+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:

     where,

Here, K_ss is a coefficient representing the odometric drift along the path traveled with respect to an incremental distance traveled, K_sθ 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,

where ν(k) is the innovation (i.e., the difference between the measurement and its expected value according to the model) and S is the matrix H_kP - _kH^T_k + R. This ε_ν(k) had a shi-square distribution, so the gate's bounding values can be read from the chi-square table, and a measurement will be discarded if its ε_ν(k) value is outside the bounding values.

Time Propagation: When a new control input vector u(k)=[ΔD_k,Δθ_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, (x_a,y_a)).
• Project the current measurement onto the xy plan.
• Using (x_a,y_a) and the known beacon location (x_a,y_a), compute H_k.
• Look up the variance R_k and the mean y_k associated with the current measurement from its stored PDF.
• Using the measurement model, compute the expected range r_k to the beacon. Let ν(k) = y_k - r_k be the innovation.
• Compute S_k = H_kP^-_kH^T_k + R_k
• Compute the Kalman gain K_k = P^-_kH^T_kS^-1_k.
• 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 - K_kS_kK^T_k.
• 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 (x_b, y_b), to the robot with state vector q_k can be expressed as:

The Jacobian H(k) gives us the measurement matrix:

                                                where,

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.