diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 4af748ec71..2119e1eacc 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -91,6 +91,29 @@ def observation_model(x): def generate_sigma_points(xEst, PEst, gamma): + """ + Generate sigma points for UKF. + + Sigma points are deterministically sampled points used to capture + the mean and covariance of the state distribution. + + Parameters + ---------- + xEst : numpy.ndarray + Current state estimate (n x 1) + PEst : numpy.ndarray + Current state covariance estimate (n x n) + gamma : float + Scaling parameter sqrt(n + lambda) + + Returns + ------- + sigma : numpy.ndarray + Sigma points (n x 2n+1) + sigma[:, 0] = xEst + sigma[:, 1:n+1] = xEst + gamma * sqrt(P) + sigma[:, n+1:2n+1] = xEst - gamma * sqrt(P) + """ sigma = xEst Psqrt = scipy.linalg.sqrtm(PEst) n = len(xEst[:, 0]) @@ -149,6 +172,35 @@ def calc_pxz(sigma, x, z_sigma, zb, wc): def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma): + """ + 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) + """ # Predict sigma = generate_sigma_points(xEst, PEst, gamma) sigma = predict_sigma_motion(sigma, u) @@ -194,6 +246,31 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover def setup_ukf(nx): + """ + Setup UKF parameters and weights. + + Calculates the weights for mean and covariance computation, + and the scaling parameter gamma for sigma point generation. + + Parameters + ---------- + nx : int + Dimension of the state vector + + Returns + ------- + wm : numpy.ndarray + Weights for calculating mean (1 x 2n+1) + wm[0] = lambda / (n + lambda) + wm[i] = 1 / (2(n + lambda)) for i > 0 + wc : numpy.ndarray + Weights for calculating covariance (1 x 2n+1) + wc[0] = lambda / (n + lambda) + (1 - alpha^2 + beta) + wc[i] = 1 / (2(n + lambda)) for i > 0 + gamma : float + Sigma point scaling parameter sqrt(n + lambda) + where lambda = alpha^2 * (n + kappa) - n + """ lamb = ALPHA ** 2 * (nx + KAPPA) - nx # calculate weights wm = [lamb / (lamb + nx)] diff --git a/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst index a7a5aab61b..01d161b17e 100644 --- a/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst +++ b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst @@ -5,7 +5,10 @@ Unscented Kalman Filter localization This is a sensor fusion localization with Unscented Kalman Filter(UKF). -The lines and points are same meaning of the EKF simulation. +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 ~~~~~~~~~~~~~ @@ -13,7 +16,242 @@ Code Link .. autofunction:: Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation +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 === + +1. Generate sigma points around the current state estimate: + + :math:`\chi_0 = \mathbf{x}_{t}` + + :math:`\chi_i = \mathbf{x}_{t} + \gamma\sqrt{\mathbf{P}_t}_i` for :math:`i = 1, ..., n` + + :math:`\chi_i = \mathbf{x}_{t} - \gamma\sqrt{\mathbf{P}_t}_{i-n}` for :math:`i = n+1, ..., 2n` + + where :math:`\gamma = \sqrt{n + \lambda}` and :math:`\lambda = \alpha^2(n + \kappa) - n` + +2. Propagate sigma points through the motion model: + + :math:`\chi^-_i = f(\chi_i, \mathbf{u}_t)` + +3. Calculate predicted state mean: + + :math:`\mathbf{x}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \chi^-_i` + +4. Calculate predicted state covariance: + + :math:`\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 === + +1. Generate sigma points around the predicted state: + + :math:`\chi_i = \text{generate\_sigma\_points}(\mathbf{x}^-_{t+1}, \mathbf{P}^-_{t+1})` + +2. Propagate sigma points through the observation model: + + :math:`\mathcal{Z}_i = h(\chi_i)` + +3. Calculate predicted observation mean: + + :math:`\mathbf{z}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \mathcal{Z}_i` + +4. Calculate innovation covariance: + + :math:`\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}` + +5. Calculate cross-correlation matrix: + + :math:`\mathbf{P}_{xz} = \sum_{i=0}^{2n} w^c_i (\chi_i - \mathbf{x}^-_{t+1})(\mathcal{Z}_i - \mathbf{z}^-_{t+1})^T` + +6. Calculate Kalman gain: + + :math:`\mathbf{K} = \mathbf{P}_{xz}\mathbf{S}_t^{-1}` + +7. Update state estimate: + + :math:`\mathbf{x}_{t+1} = \mathbf{x}^-_{t+1} + \mathbf{K}(\mathbf{z}_t - \mathbf{z}^-_{t+1})` + +8. Update covariance estimate: + + :math:`\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:** + +:math:`w^m_0 = \frac{\lambda}{n + \lambda}` + +:math:`w^m_i = \frac{1}{2(n + \lambda)}` for :math:`i = 1, ..., 2n` + +**Covariance weights:** + +:math:`w^c_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)` + +:math:`w^c_i = \frac{1}{2(n + \lambda)}` for :math:`i = 1, ..., 2n` + +where: + +- :math:`\alpha` controls the spread of sigma points around the mean (typically :math:`0.001 \leq \alpha \leq 1`) +- :math:`\beta` incorporates prior knowledge of the distribution (:math:`\beta = 2` is optimal for Gaussian distributions) +- :math:`\kappa` is a secondary scaling parameter (typically :math:`\kappa = 0`) +- :math:`n` is the dimension of the state vector + + +Filter Design +~~~~~~~~~~~~~ + +In this simulation, the robot has a state vector with 4 states at time :math:`t`: + +.. math:: \mathbf{x}_t = [x_t, y_t, \phi_t, v_t]^T + +where: + +- :math:`x, y` are 2D position coordinates +- :math:`\phi` is orientation (yaw angle) +- :math:`v` is velocity + +In the code, "xEst" means the state vector. + +The covariance matrices are: + +- :math:`\mathbf{P}_t` is the state covariance matrix +- :math:`\mathbf{Q}` is the process noise covariance matrix +- :math:`\mathbf{R}` is the observation noise covariance matrix + +**Process Noise Covariance** :math:`\mathbf{Q}`: + +.. math:: + \mathbf{Q} = \begin{bmatrix} + 0.1^2 & 0 & 0 & 0 \\ + 0 & 0.1^2 & 0 & 0 \\ + 0 & 0 & (0.017)^2 & 0 \\ + 0 & 0 & 0 & 1.0^2 + \end{bmatrix} + +**Observation Noise Covariance** :math:`\mathbf{R}`: + +.. math:: + \mathbf{R} = \begin{bmatrix} + 1.0^2 & 0 \\ + 0 & 1.0^2 + \end{bmatrix} + + +Input Vector +^^^^^^^^^^^^ + +The robot has a velocity sensor and a gyro sensor, so the control input vector at each time step is: + +.. math:: \mathbf{u}_t = [v_t, \omega_t]^T + +where: + +- :math:`v_t` is linear velocity [m/s] +- :math:`\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: + +.. math:: \mathbf{z}_t = [x_t, y_t]^T + +The observation includes GPS noise with covariance :math:`\mathbf{R}`. + + +Motion Model +~~~~~~~~~~~~ + +The robot motion model is: + +.. math:: \dot{x} = v \cos(\phi) + +.. math:: \dot{y} = v \sin(\phi) + +.. math:: \dot{\phi} = \omega + +.. math:: \dot{v} = 0 + +Discretized with time step :math:`\Delta t`, the motion model becomes: + +.. math:: \mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) = \mathbf{F}\mathbf{x}_t + \mathbf{B}\mathbf{u}_t + +where: + +:math:`\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*}` + +:math:`\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: + +:math:`\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*}` + +:math:`\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: + +.. math:: \mathbf{z}_{t} = h(\mathbf{x}_t) = \mathbf{H} \mathbf{x}_t + +where: + +:math:`\begin{equation*} \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix} \end{equation*}` + +The observation function states that: + +:math:`\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 :math:`\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 + + Reference ~~~~~~~~~~~ - `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization `_ +- `The Unscented Kalman Filter for Nonlinear Estimation `_ +- `PROBABILISTIC ROBOTICS `_