Unscented Kalman Filter localization
This is a sensor fusion localization with Unscented Kalman Filter(UKF).
The blue line is true trajectory, the black line is dead reckoning trajectory, the green points are GPS observations, and the red line is estimated trajectory with UKF.
The red ellipse is estimated covariance ellipse with UKF.
Code Link
- Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation(xEst, PEst, z, u, wm, wc, gamma)[source]
Unscented Kalman Filter estimation.
Performs one iteration of UKF state estimation using the unscented transform.
- Parameters:
xEst (numpy.ndarray) – Current state estimate [x, y, yaw, v]^T (4 x 1)
PEst (numpy.ndarray) – Current state covariance estimate (4 x 4)
z (numpy.ndarray) – Observation vector [x_obs, y_obs]^T (2 x 1)
u (numpy.ndarray) – Control input [velocity, yaw_rate]^T (2 x 1)
wm (numpy.ndarray) – Weights for calculating mean (1 x 2n+1)
wc (numpy.ndarray) – Weights for calculating covariance (1 x 2n+1)
gamma (float) – Sigma point scaling parameter sqrt(n + lambda)
- Returns:
xEst (numpy.ndarray) – Updated state estimate (4 x 1)
PEst (numpy.ndarray) – Updated state covariance estimate (4 x 4)
Unscented Kalman Filter Algorithm
The Unscented Kalman Filter (UKF) is a nonlinear state estimation technique that uses the unscented transform to handle nonlinearities. Unlike the Extended Kalman Filter (EKF) that linearizes the nonlinear functions using Jacobians, UKF uses a deterministic sampling approach called sigma points to capture the mean and covariance of the state distribution.
The UKF algorithm consists of two main steps:
=== Predict ===
Generate sigma points around the current state estimate:
\(\chi_0 = \mathbf{x}_{t}\)
\(\chi_i = \mathbf{x}_{t} + \gamma\sqrt{\mathbf{P}_t}_i\) for \(i = 1, ..., n\)
\(\chi_i = \mathbf{x}_{t} - \gamma\sqrt{\mathbf{P}_t}_{i-n}\) for \(i = n+1, ..., 2n\)
where \(\gamma = \sqrt{n + \lambda}\) and \(\lambda = \alpha^2(n + \kappa) - n\)
Propagate sigma points through the motion model:
\(\chi^-_i = f(\chi_i, \mathbf{u}_t)\)
Calculate predicted state mean:
\(\mathbf{x}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \chi^-_i\)
Calculate predicted state covariance:
\(\mathbf{P}^-_{t+1} = \sum_{i=0}^{2n} w^c_i (\chi^-_i - \mathbf{x}^-_{t+1})(\chi^-_i - \mathbf{x}^-_{t+1})^T + \mathbf{Q}\)
=== Update ===
Generate sigma points around the predicted state:
\(\chi_i = \text{generate\_sigma\_points}(\mathbf{x}^-_{t+1}, \mathbf{P}^-_{t+1})\)
Propagate sigma points through the observation model:
\(\mathcal{Z}_i = h(\chi_i)\)
Calculate predicted observation mean:
\(\mathbf{z}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \mathcal{Z}_i\)
Calculate innovation covariance:
\(\mathbf{S}_t = \sum_{i=0}^{2n} w^c_i (\mathcal{Z}_i - \mathbf{z}^-_{t+1})(\mathcal{Z}_i - \mathbf{z}^-_{t+1})^T + \mathbf{R}\)
Calculate cross-correlation matrix:
\(\mathbf{P}_{xz} = \sum_{i=0}^{2n} w^c_i (\chi_i - \mathbf{x}^-_{t+1})(\mathcal{Z}_i - \mathbf{z}^-_{t+1})^T\)
Calculate Kalman gain:
\(\mathbf{K} = \mathbf{P}_{xz}\mathbf{S}_t^{-1}\)
Update state estimate:
\(\mathbf{x}_{t+1} = \mathbf{x}^-_{t+1} + \mathbf{K}(\mathbf{z}_t - \mathbf{z}^-_{t+1})\)
Update covariance estimate:
\(\mathbf{P}_{t+1} = \mathbf{P}^-_{t+1} - \mathbf{K}\mathbf{S}_t\mathbf{K}^T\)
Sigma Points and Weights
The UKF uses deterministic sigma points to represent the state distribution. The weights for combining sigma points are calculated as:
Mean weights:
\(w^m_0 = \frac{\lambda}{n + \lambda}\)
\(w^m_i = \frac{1}{2(n + \lambda)}\) for \(i = 1, ..., 2n\)
Covariance weights:
\(w^c_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\)
\(w^c_i = \frac{1}{2(n + \lambda)}\) for \(i = 1, ..., 2n\)
where:
\(\alpha\) controls the spread of sigma points around the mean (typically \(0.001 \leq \alpha \leq 1\))
\(\beta\) incorporates prior knowledge of the distribution (\(\beta = 2\) is optimal for Gaussian distributions)
\(\kappa\) is a secondary scaling parameter (typically \(\kappa = 0\))
\(n\) is the dimension of the state vector
Filter Design
In this simulation, the robot has a state vector with 4 states at time \(t\):
where:
\(x, y\) are 2D position coordinates
\(\phi\) is orientation (yaw angle)
\(v\) is velocity
In the code, “xEst” means the state vector.
The covariance matrices are:
\(\mathbf{P}_t\) is the state covariance matrix
\(\mathbf{Q}\) is the process noise covariance matrix
\(\mathbf{R}\) is the observation noise covariance matrix
Process Noise Covariance \(\mathbf{Q}\):
Observation Noise Covariance \(\mathbf{R}\):
Input Vector
The robot has a velocity sensor and a gyro sensor, so the control input vector at each time step is:
where:
\(v_t\) is linear velocity [m/s]
\(\omega_t\) is angular velocity (yaw rate) [rad/s]
The input vector includes sensor noise.
Observation Vector
The robot has a GNSS (GPS) sensor that can measure x-y position:
The observation includes GPS noise with covariance \(\mathbf{R}\).
Motion Model
The robot motion model is:
Discretized with time step \(\Delta t\), the motion model becomes:
where:
\(\begin{equation*} \mathbf{F} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}\)
\(\begin{equation*} \mathbf{B} = \begin{bmatrix} \cos(\phi) \Delta t & 0\\ \sin(\phi) \Delta t & 0\\ 0 & \Delta t\\ 1 & 0\\ \end{bmatrix} \end{equation*}\)
The motion function expands to:
\(\begin{equation*} \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \phi_{t+1} \\ v_{t+1} \end{bmatrix} = \begin{bmatrix} x_t + v_t\cos(\phi_t)\Delta t \\ y_t + v_t\sin(\phi_t)\Delta t \\ \phi_t + \omega_t \Delta t \\ v_t \end{bmatrix} \end{equation*}\)
\(\Delta t = 0.1\) [s] is the time interval.
Observation Model
The robot can get x-y position information from GPS.
The GPS observation model is:
where:
\(\begin{equation*} \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix} \end{equation*}\)
The observation function states that:
\(\begin{equation*} \begin{bmatrix} x_{obs} \\ y_{obs} \end{bmatrix} = h(\mathbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}\)
UKF Parameters
The UKF uses three tuning parameters:
ALPHA = 0.001: Controls the spread of sigma points around the mean. Smaller values result in sigma points closer to the mean.
BETA = 2: Incorporates prior knowledge about the distribution. For Gaussian distributions, the optimal value is 2.
KAPPA = 0: Secondary scaling parameter. Usually set to 0 or 3-n where n is the state dimension.
These parameters affect the calculation of \(\lambda = \alpha^2(n + \kappa) - n\), which determines the sigma point spread and weights.
Advantages of UKF over EKF
The Unscented Kalman Filter offers several advantages over the Extended Kalman Filter:
No Jacobian required: UKF does not need to compute Jacobian matrices, which can be error-prone and computationally expensive for complex nonlinear systems
Higher accuracy: UKF captures the mean and covariance to the second order (third order for Gaussian distributions) while EKF only achieves first-order accuracy
Better handling of nonlinearities: The unscented transform provides a better approximation of the probability distribution after nonlinear transformation
Easier implementation: For highly nonlinear systems, UKF is often easier to implement since it doesn’t require analytical derivatives
Numerical stability: UKF can be more numerically stable than EKF for strongly nonlinear systems