Project 1 - UKF vs EKF for a Practical Robot Localization Problem
PROJECT 1
Define the state variables: Determine the state variables that describe the system. These variables will change over time as the system evolves.
Define the state transition model: Develop a mathematical model that describes how the state variables change over time. This is the state transition model. In the case of the automobile localization problem, the state transition model is the bicycle model for the motion of the automobile.
Define the measurement model: Develop a mathematical model that describes how sensor measurements relate to the state variables. In the case of the automobile localization problem, the measurement model is the landmark-based approach that uses range and bearing sensor measurements.
Implement the filter: Use the state transition model and measurement model to implement the EKF or UKF algorithm. This typically involves predicting the state based on the previous state and measurement, and then updating the state based on the latest measurement.
Tune the filter: The filter may require tuning of various parameters, such as the process noise covariance and measurement noise covariance, to achieve optimal performance.
Evaluate the filter: Evaluate the performance of the filter by analyzing the estimated state trajectory and comparing it to ground truth or other benchmarks. Performance metrics may include convergence rate, accuracy, and computational efficiency.
Implementation of EKF:
Initialize state vector and covariance matrix
Predict step:
Compute the state transition matrix and propagate the state vector using the bicycle model equations.
Compute the Jacobian of the state transition matrix and the process noise covariance matrix.
Propagate the covariance matrix using the computed Jacobian and process noise covariance matrix.
Update step:
Compute the measurement model Jacobian and measurement noise covariance matrix.
Compute the Kalman gain matrix using the predicted covariance matrix and measurement noise covariance matrix.
Compute the innovation and update the state vector and covariance matrix.
Implementation of UKF:
Initialize state vector and covariance matrix
Predict step:
Compute the sigma points based on the state vector and covariance matrix.
Propagate the sigma points using the bicycle model equations.
Compute the predicted mean and covariance matrix based on the propagated sigma points.
Update step:
Compute the sigma points based on the predicted mean and covariance matrix.
Compute the measurement model for each sigma point and the predicted mean and covariance matrix.
Compute the Kalman gain matrix using the predicted covariance matrix and measurement noise covariance matrix.
Compute the innovation and update the state vector and covariance matrix.
To compare the performance of EKF and UKF, you can use the following metrics:
Consistency: Check if the estimated state converges to the true state in the absence of measurement noise.
Accuracy: Check the difference between the estimated state and the true state for a given test case.
To evaluate the UKF and EKF filters, we can use the following parameters:
Divergence: The filter should not diverge, which means the estimated position and orientation of the robot should not deviate too far from the ground truth trajectory. We can measure the error between the ground truth trajectory and the estimated trajectory and ensure that the error remains within acceptable limits.
Orientation error: We can compare the estimated orientation of the robot with the ground truth orientation and ensure that the error remains within acceptable limits.
Convergence rate: We can compare the convergence rates of the UKF and EKF filters and determine which filter converges faster for a given test case.
To perform these evaluations, we can run both the UKF and EKF filters on several test cases and compare their performance based on the above criteria. We can plot the trajectories generated by both filters and compare them to the ground truth trajectory to evaluate the divergence of the filters. We can also plot the orientation estimates generated by both filters and compare them to the ground truth orientation to evaluate the orientation error. Additionally, we can measure the convergence rate and runtime performance of both filters and compare them to determine which filter is better suited for the given problem.
Unscented Kalman Filter (UKF) is known to offer better performance than Extended Kalman Filter (EKF). UKF uses a deterministic sampling approach, known as unscented transformation, to compute the predicted mean and covariance of the state, whereas EKF uses a linear approximation of the nonlinear functions.
UKF can handle non-Gaussian noise processes in a more robust manner. UKF can estimate the state even when the noise processes have non-zero means and non-constant covariances. EKF, on the other hand, requires the noise processes to be Gaussian with zero mean and constant covariance, which is a significant limitation in practical applications.