Skip to content

Commit fc6b36e

Browse files
CopilotAtsushiSakai
Copilot
and
AtsushiSakai
authored
Add comprehensive documentation for Unscented Kalman Filter localization (#1290)
* Initial plan * Add comprehensive UKF documentation with algorithm details Co-authored-by: AtsushiSakai <[email protected]> * Add comprehensive docstrings to UKF core functions Co-authored-by: AtsushiSakai <[email protected]> * Fix codestyle issues - remove trailing whitespace from docstrings Co-authored-by: AtsushiSakai <[email protected]> --------- Co-authored-by: copilot-swe-agent[bot] <[email protected]> Co-authored-by: AtsushiSakai <[email protected]>
1 parent 39e29bb commit fc6b36e

File tree

2 files changed

+316
-1
lines changed

2 files changed

+316
-1
lines changed

‎Localization/unscented_kalman_filter/unscented_kalman_filter.py‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,29 @@ def observation_model(x):
9191

9292

9393
def generate_sigma_points(xEst, PEst, gamma):
94+
"""
95+
Generate sigma points for UKF.
96+
97+
Sigma points are deterministically sampled points used to capture
98+
the mean and covariance of the state distribution.
99+
100+
Parameters
101+
----------
102+
xEst : numpy.ndarray
103+
Current state estimate (n x 1)
104+
PEst : numpy.ndarray
105+
Current state covariance estimate (n x n)
106+
gamma : float
107+
Scaling parameter sqrt(n + lambda)
108+
109+
Returns
110+
-------
111+
sigma : numpy.ndarray
112+
Sigma points (n x 2n+1)
113+
sigma[:, 0] = xEst
114+
sigma[:, 1:n+1] = xEst + gamma * sqrt(P)
115+
sigma[:, n+1:2n+1] = xEst - gamma * sqrt(P)
116+
"""
94117
sigma = xEst
95118
Psqrt = scipy.linalg.sqrtm(PEst)
96119
n = len(xEst[:, 0])
@@ -149,6 +172,35 @@ def calc_pxz(sigma, x, z_sigma, zb, wc):
149172

150173

151174
def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
175+
"""
176+
Unscented Kalman Filter estimation.
177+
178+
Performs one iteration of UKF state estimation using the unscented transform.
179+
180+
Parameters
181+
----------
182+
xEst : numpy.ndarray
183+
Current state estimate [x, y, yaw, v]^T (4 x 1)
184+
PEst : numpy.ndarray
185+
Current state covariance estimate (4 x 4)
186+
z : numpy.ndarray
187+
Observation vector [x_obs, y_obs]^T (2 x 1)
188+
u : numpy.ndarray
189+
Control input [velocity, yaw_rate]^T (2 x 1)
190+
wm : numpy.ndarray
191+
Weights for calculating mean (1 x 2n+1)
192+
wc : numpy.ndarray
193+
Weights for calculating covariance (1 x 2n+1)
194+
gamma : float
195+
Sigma point scaling parameter sqrt(n + lambda)
196+
197+
Returns
198+
-------
199+
xEst : numpy.ndarray
200+
Updated state estimate (4 x 1)
201+
PEst : numpy.ndarray
202+
Updated state covariance estimate (4 x 4)
203+
"""
152204
# Predict
153205
sigma = generate_sigma_points(xEst, PEst, gamma)
154206
sigma = predict_sigma_motion(sigma, u)
@@ -194,6 +246,31 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
194246

195247

196248
def setup_ukf(nx):
249+
"""
250+
Setup UKF parameters and weights.
251+
252+
Calculates the weights for mean and covariance computation,
253+
and the scaling parameter gamma for sigma point generation.
254+
255+
Parameters
256+
----------
257+
nx : int
258+
Dimension of the state vector
259+
260+
Returns
261+
-------
262+
wm : numpy.ndarray
263+
Weights for calculating mean (1 x 2n+1)
264+
wm[0] = lambda / (n + lambda)
265+
wm[i] = 1 / (2(n + lambda)) for i > 0
266+
wc : numpy.ndarray
267+
Weights for calculating covariance (1 x 2n+1)
268+
wc[0] = lambda / (n + lambda) + (1 - alpha^2 + beta)
269+
wc[i] = 1 / (2(n + lambda)) for i > 0
270+
gamma : float
271+
Sigma point scaling parameter sqrt(n + lambda)
272+
where lambda = alpha^2 * (n + kappa) - n
273+
"""
197274
lamb = ALPHA ** 2 * (nx + KAPPA) - nx
198275
# calculate weights
199276
wm = [lamb / (lamb + nx)]

‎docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst‎

Lines changed: 239 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,15 +5,253 @@ Unscented Kalman Filter localization
55

66
This is a sensor fusion localization with Unscented Kalman Filter(UKF).
77

8-
The lines and points are same meaning of the EKF simulation.
8+
The blue line is true trajectory, the black line is dead reckoning trajectory,
9+
the green points are GPS observations, and the red line is estimated trajectory with UKF.
10+
11+
The red ellipse is estimated covariance ellipse with UKF.
912

1013
Code Link
1114
~~~~~~~~~~~~~
1215

1316
.. autofunction:: Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation
1417

1518

19+
Unscented Kalman Filter Algorithm
20+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
21+
22+
The Unscented Kalman Filter (UKF) is a nonlinear state estimation technique that uses
23+
the unscented transform to handle nonlinearities. Unlike the Extended Kalman Filter (EKF)
24+
that linearizes the nonlinear functions using Jacobians, UKF uses a deterministic sampling
25+
approach called sigma points to capture the mean and covariance of the state distribution.
26+
27+
The UKF algorithm consists of two main steps:
28+
29+
=== Predict ===
30+
31+
1. Generate sigma points around the current state estimate:
32+
33+
:math:`\chi_0 = \mathbf{x}_{t}`
34+
35+
:math:`\chi_i = \mathbf{x}_{t} + \gamma\sqrt{\mathbf{P}_t}_i` for :math:`i = 1, ..., n`
36+
37+
:math:`\chi_i = \mathbf{x}_{t} - \gamma\sqrt{\mathbf{P}_t}_{i-n}` for :math:`i = n+1, ..., 2n`
38+
39+
where :math:`\gamma = \sqrt{n + \lambda}` and :math:`\lambda = \alpha^2(n + \kappa) - n`
40+
41+
2. Propagate sigma points through the motion model:
42+
43+
:math:`\chi^-_i = f(\chi_i, \mathbf{u}_t)`
44+
45+
3. Calculate predicted state mean:
46+
47+
:math:`\mathbf{x}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \chi^-_i`
48+
49+
4. Calculate predicted state covariance:
50+
51+
: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}`
52+
53+
=== Update ===
54+
55+
1. Generate sigma points around the predicted state:
56+
57+
:math:`\chi_i = \text{generate\_sigma\_points}(\mathbf{x}^-_{t+1}, \mathbf{P}^-_{t+1})`
58+
59+
2. Propagate sigma points through the observation model:
60+
61+
:math:`\mathcal{Z}_i = h(\chi_i)`
62+
63+
3. Calculate predicted observation mean:
64+
65+
:math:`\mathbf{z}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \mathcal{Z}_i`
66+
67+
4. Calculate innovation covariance:
68+
69+
: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}`
70+
71+
5. Calculate cross-correlation matrix:
72+
73+
: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`
74+
75+
6. Calculate Kalman gain:
76+
77+
:math:`\mathbf{K} = \mathbf{P}_{xz}\mathbf{S}_t^{-1}`
78+
79+
7. Update state estimate:
80+
81+
:math:`\mathbf{x}_{t+1} = \mathbf{x}^-_{t+1} + \mathbf{K}(\mathbf{z}_t - \mathbf{z}^-_{t+1})`
82+
83+
8. Update covariance estimate:
84+
85+
:math:`\mathbf{P}_{t+1} = \mathbf{P}^-_{t+1} - \mathbf{K}\mathbf{S}_t\mathbf{K}^T`
86+
87+
88+
Sigma Points and Weights
89+
~~~~~~~~~~~~~~~~~~~~~~~~~
90+
91+
The UKF uses deterministic sigma points to represent the state distribution. The weights
92+
for combining sigma points are calculated as:
93+
94+
**Mean weights:**
95+
96+
:math:`w^m_0 = \frac{\lambda}{n + \lambda}`
97+
98+
:math:`w^m_i = \frac{1}{2(n + \lambda)}` for :math:`i = 1, ..., 2n`
99+
100+
**Covariance weights:**
101+
102+
:math:`w^c_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)`
103+
104+
:math:`w^c_i = \frac{1}{2(n + \lambda)}` for :math:`i = 1, ..., 2n`
105+
106+
where:
107+
108+
- :math:`\alpha` controls the spread of sigma points around the mean (typically :math:`0.001 \leq \alpha \leq 1`)
109+
- :math:`\beta` incorporates prior knowledge of the distribution (:math:`\beta = 2` is optimal for Gaussian distributions)
110+
- :math:`\kappa` is a secondary scaling parameter (typically :math:`\kappa = 0`)
111+
- :math:`n` is the dimension of the state vector
112+
113+
114+
Filter Design
115+
~~~~~~~~~~~~~
116+
117+
In this simulation, the robot has a state vector with 4 states at time :math:`t`:
118+
119+
.. math:: \mathbf{x}_t = [x_t, y_t, \phi_t, v_t]^T
120+
121+
where:
122+
123+
- :math:`x, y` are 2D position coordinates
124+
- :math:`\phi` is orientation (yaw angle)
125+
- :math:`v` is velocity
126+
127+
In the code, "xEst" means the state vector.
128+
129+
The covariance matrices are:
130+
131+
- :math:`\mathbf{P}_t` is the state covariance matrix
132+
- :math:`\mathbf{Q}` is the process noise covariance matrix
133+
- :math:`\mathbf{R}` is the observation noise covariance matrix
134+
135+
**Process Noise Covariance** :math:`\mathbf{Q}`:
136+
137+
.. math::
138+
\mathbf{Q} = \begin{bmatrix}
139+
0.1^2 & 0 & 0 & 0 \\
140+
0 & 0.1^2 & 0 & 0 \\
141+
0 & 0 & (0.017)^2 & 0 \\
142+
0 & 0 & 0 & 1.0^2
143+
\end{bmatrix}
144+
145+
**Observation Noise Covariance** :math:`\mathbf{R}`:
146+
147+
.. math::
148+
\mathbf{R} = \begin{bmatrix}
149+
1.0^2 & 0 \\
150+
0 & 1.0^2
151+
\end{bmatrix}
152+
153+
154+
Input Vector
155+
^^^^^^^^^^^^
156+
157+
The robot has a velocity sensor and a gyro sensor, so the control input vector at each time step is:
158+
159+
.. math:: \mathbf{u}_t = [v_t, \omega_t]^T
160+
161+
where:
162+
163+
- :math:`v_t` is linear velocity [m/s]
164+
- :math:`\omega_t` is angular velocity (yaw rate) [rad/s]
165+
166+
The input vector includes sensor noise.
167+
168+
169+
Observation Vector
170+
^^^^^^^^^^^^^^^^^^
171+
172+
The robot has a GNSS (GPS) sensor that can measure x-y position:
173+
174+
.. math:: \mathbf{z}_t = [x_t, y_t]^T
175+
176+
The observation includes GPS noise with covariance :math:`\mathbf{R}`.
177+
178+
179+
Motion Model
180+
~~~~~~~~~~~~
181+
182+
The robot motion model is:
183+
184+
.. math:: \dot{x} = v \cos(\phi)
185+
186+
.. math:: \dot{y} = v \sin(\phi)
187+
188+
.. math:: \dot{\phi} = \omega
189+
190+
.. math:: \dot{v} = 0
191+
192+
Discretized with time step :math:`\Delta t`, the motion model becomes:
193+
194+
.. math:: \mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) = \mathbf{F}\mathbf{x}_t + \mathbf{B}\mathbf{u}_t
195+
196+
where:
197+
198+
: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*}`
199+
200+
: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*}`
201+
202+
The motion function expands to:
203+
204+
: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*}`
205+
206+
:math:`\Delta t = 0.1` [s] is the time interval.
207+
208+
209+
Observation Model
210+
~~~~~~~~~~~~~~~~~
211+
212+
The robot can get x-y position information from GPS.
213+
214+
The GPS observation model is:
215+
216+
.. math:: \mathbf{z}_{t} = h(\mathbf{x}_t) = \mathbf{H} \mathbf{x}_t
217+
218+
where:
219+
220+
:math:`\begin{equation*} \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix} \end{equation*}`
221+
222+
The observation function states that:
223+
224+
:math:`\begin{equation*} \begin{bmatrix} x_{obs} \\ y_{obs} \end{bmatrix} = h(\mathbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}`
225+
226+
227+
UKF Parameters
228+
~~~~~~~~~~~~~~
229+
230+
The UKF uses three tuning parameters:
231+
232+
- **ALPHA = 0.001**: Controls the spread of sigma points around the mean. Smaller values result in sigma points closer to the mean.
233+
- **BETA = 2**: Incorporates prior knowledge about the distribution. For Gaussian distributions, the optimal value is 2.
234+
- **KAPPA = 0**: Secondary scaling parameter. Usually set to 0 or 3-n where n is the state dimension.
235+
236+
These parameters affect the calculation of :math:`\lambda = \alpha^2(n + \kappa) - n`, which determines
237+
the sigma point spread and weights.
238+
239+
240+
Advantages of UKF over EKF
241+
~~~~~~~~~~~~~~~~~~~~~~~~~~~
242+
243+
The Unscented Kalman Filter offers several advantages over the Extended Kalman Filter:
244+
245+
- **No Jacobian required**: UKF does not need to compute Jacobian matrices, which can be error-prone and computationally expensive for complex nonlinear systems
246+
- **Higher accuracy**: UKF captures the mean and covariance to the second order (third order for Gaussian distributions) while EKF only achieves first-order accuracy
247+
- **Better handling of nonlinearities**: The unscented transform provides a better approximation of the probability distribution after nonlinear transformation
248+
- **Easier implementation**: For highly nonlinear systems, UKF is often easier to implement since it doesn't require analytical derivatives
249+
- **Numerical stability**: UKF can be more numerically stable than EKF for strongly nonlinear systems
250+
251+
16252
Reference
17253
~~~~~~~~~~~
18254

19255
- `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization <https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization>`_
256+
- `The Unscented Kalman Filter for Nonlinear Estimation <https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf>`_
257+
- `PROBABILISTIC ROBOTICS <http://www.probabilistic-robotics.org/>`_

0 commit comments

Comments
 (0)