Models¶
This page describes the models considered in the examples. Each class contains propagation and measurement functions, different choices of retractions and their inverse retractions. You can also obtain in the source code useful functions specifically related to the problem, e.g. Jacobian for EKF, and helper functions.
2D Robot Localization¶
-
class
ukfm.LOCALIZATION(T, odo_freq)[source]¶ 2D Robot localization based on odometry and GNSS (robot position) measurements. See a text description in [BB17], Section IV.
- Variables
T – sequence time (s).
odo_freq – odometry frequency (Hz).
-
class
STATE(Rot, p)[source]¶ State of the system.
It represents the orientation and the position of the robot.
\[ \boldsymbol{\chi} \in \mathcal{M} = \left\{ \begin{matrix} \mathbf{C} \in SO(2), \mathbf{p} \in \mathbb R^2 \end{matrix} \right\}\]- Variables
Rot – rotation matrix \(\mathbf{C}\).
p – position vector \(\mathbf{p}\).
-
class
INPUT(v, gyro)[source]¶ Input of the propagation model.
The input are the robot velocities that can be obtained from a differential wheel system.
\[\boldsymbol{\omega} \in \mathcal{U} = \left\{ \begin{matrix} \mathbf{v} \in \mathbb R^2, \omega \in \mathbb R \end{matrix} \right\}\]- Variables
v – robot forward and lateral velocities \(\mathbf{v}\).
gyro – robot orientation velocity \(\omega\).
-
classmethod
f(state, omega, w, dt)[source]¶ Propagation function.
\[\begin{split}\mathbf{C}_{n+1} &= \mathbf{C}_{n} \exp\left(\left(\omega + \mathbf{w}^{(2)} \right) dt\right) \\ \mathbf{p}_{n+1} &= \mathbf{p}_{n} + \left( \mathbf{v}_{n} + \mathbf{w}^{(0:2)} \right) dt\end{split}\]- Variables
state – state \(\boldsymbol{\chi}\).
omega – input \(\boldsymbol{\omega}\).
w – noise \(\mathbf{w}\).
dt – integration step \(dt\) (s).
-
classmethod
h(state)[source]¶ Observation function.
\[h\left(\boldsymbol{\chi}\right) = \mathbf{p}\]- Variables
state – state \(\boldsymbol{\chi}\).
-
classmethod
phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C} \exp\left(\boldsymbol{\xi}^{(0)}\right) \\ \mathbf{p} + \boldsymbol{\xi}^{(1:3)} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(2) \times \mathbb R^2\).
Its corresponding inverse operation is
phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\begin{split}\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \left( \begin{matrix} \log\left(\mathbf{C} \mathbf{\hat{C}}^T\right) \\ \mathbf{p} - \mathbf{\hat{p}} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(2) \times \mathbb R^2\).
Its corresponding retraction is
phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
left_phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C} \mathbf{C}_\mathbf{T} \\ \mathbf{p} + \mathbf{C} \mathbf{r} \end{matrix} \right)\end{split}\]where
\[\begin{split}\mathbf{T} = \exp\left(\boldsymbol{\xi}\right) = \begin{bmatrix} \mathbf{C}_\mathbf{T} & \mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix}\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE(2)\) with left multiplication.
Its corresponding inverse operation is
left_phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
left_phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \log\left( \boldsymbol{\chi}^{-1} \boldsymbol{\hat{\chi}} \right)\]The state is viewed as a element \(\boldsymbol{\chi} \in SE(2)\) with left multiplication.
Its corresponding retraction is
left_phi().- Parameters
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
right_phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C}_\mathbf{T} \mathbf{C} \\ \mathbf{C}_\mathbf{T}\mathbf{p} + \mathbf{r} \end{matrix} \right)\end{split}\]where
\[\begin{split}\mathbf{T} = \exp\left(\boldsymbol{\xi}\right) = \begin{bmatrix} \mathbf{C}_\mathbf{T} &\mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix}\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE(2)\) with right multiplication.
Its corresponding inverse operation is
right_phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
right_phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \log\left( \boldsymbol{\hat{\chi}}^{-1} \boldsymbol{\chi} \right)\]The state is viewed as a element \(\boldsymbol{\chi} \in SE(2)\) with right multiplication.
Its corresponding retraction is
right_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
3D Attitude Estimation with an IMU¶
-
class
ukfm.ATTITUDE(T, imu_freq)[source]¶ 3D attitude estimation from an IMU equipped with gyro, accelerometer and magnetometer. See text description in [KHSchon17], Section IV.
- Parameters
T – sequence time (s).
imu_freq – IMU frequency (Hz).
-
g= array([ 0. , 0. , -9.82])¶ gravity vector (m/s^2) \(\mathbf{g}\).
-
b= array([ 0.33, 0. , -0.95])¶ normed magnetic field in Sweden \(\mathbf{b}\).
-
class
STATE(Rot)[source]¶ State of the system.
It represents the orientation of the platform.
\[ \boldsymbol{\chi} \in \mathcal{M} = \left\{ \mathbf{C} \in SO(3) \right\}\]- Variables
Rot – rotation matrix \(\mathbf{C}\).
-
class
INPUT(gyro)[source]¶ Input of the propagation model.
The input is the gyro measurement from an Inertial Measurement Unit (IMU).
\[\boldsymbol{\omega} \in \mathcal{U} = \left\{ \mathbf{u} \in \mathbb R^3 \right\}\]- Variables
gyro – 3D gyro \(\mathbf{u}\).
-
classmethod
f(state, omega, w, dt)[source]¶ Propagation function.
\[\mathbf{C}_{n+1} = \mathbf{C}_{n} \exp\left(\left(\mathbf{u} + \mathbf{w} \right) dt \right)\]- Variables
state – state \(\boldsymbol{\chi}\).
omega – input \(\boldsymbol{\omega}\).
w – noise \(\mathbf{w}\).
dt – integration step \(dt\) (s).
-
classmethod
h(state)[source]¶ Observation function.
\[\begin{split}h\left(\boldsymbol{\chi}\right) = \begin{bmatrix} \mathbf{C}^T \mathbf{g} \\ \mathbf{C}^T \mathbf{b} \end{bmatrix}\end{split}\]- Variables
state – state \(\boldsymbol{\chi}\).
-
classmethod
phi(state, xi)[source]¶ Retraction.
\[\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \mathbf{C} \exp\left(\boldsymbol{\xi}\right)\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3)\) with left multiplication.
Its corresponding inverse operation is
phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \log\left( \boldsymbol{\chi}^{-1} \boldsymbol{\hat{\chi}} \right)\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3)\) with left multiplication.
Its corresponding retraction is
phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
right_phi(state, xi)[source]¶ Retraction.
\[\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \exp\left(\boldsymbol{\xi}\right) \mathbf{C} \]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3)\) with right multiplication.
Its corresponding inverse operation is
right_phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
right_phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \log\left( \boldsymbol{\hat{\chi}}\boldsymbol{\chi}^{-1} \right)\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3)\) with right multiplication.
Its corresponding retraction is
right_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
2D Robot SLAM¶
-
class
ukfm.SLAM2D(T, odo_freq)[source]¶ 2D SLAM based on robot odometry and unknown landmark position measurements. See a description of the model in the two references [HMR10] , [HMR13].
- Variables
T – sequence time (s).
odo_freq – odometry frequency (Hz).
-
max_range= 5¶ maximal range of observation (m).
-
min_range= 1¶ minimal range of observation (m).
-
N_ldk= 20¶ number of landmarks \(L\).
-
class
STATE(Rot, p, p_l=array([], shape=(2, 0), dtype=float64))[source]¶ State of the system.
It represents the orientation and the position of the robot along with yet observed landmarks.
\[ \boldsymbol{\chi} \in \mathcal{M} = \left\{ \begin{matrix} \mathbf{C} \in SO(2), \mathbf{p} \in \mathbb R^2, \mathbf{p}^l_1 \in \mathbb R^2, \ldots, \mathbf{p}^l_L \in \mathbb R^2 \end{matrix} \right\}\]- Variables
Rot – rotation matrix \(\mathbf{C}\).
p – position of the robot \(\mathbf{p}\).
p_l – position of the landmark \(\mathbf{p}^l_1, \ldots, \mathbf{p}^l_L\).
-
class
INPUT(v, gyro)[source]¶ Input of the propagation model.
The input are the robot velocities that can be obtained from a differential wheel system.
\[\boldsymbol{\omega} \in \mathcal{U} = \left\{ \begin{matrix} \mathbf{v} \in \mathbb R, \omega \in \mathbb R \end{matrix} \right\}\]- Variables
v – robot forward velocity \(v\).
gyro – robot orientation velocity \(\omega\).
-
classmethod
f(state, omega, w, dt)[source]¶ Propagation function.
\[\begin{split}\mathbf{C}_{n+1} &= \mathbf{C}_{n} \exp\left(\left(\omega + \mathbf{w}^{(1)} \right) dt\right) \\ \mathbf{p}_{n+1} &= \mathbf{p}_{n} + \left( \mathbf{v}_{n} + \mathbf{w}^{(0)} \right) dt \\ \mathbf{p}_{1,n+1}^l &= \mathbf{p}_{1,n}^l \\ \vdots \\ \mathbf{p}_{L,n+1}^l &= \mathbf{p}_{L,n}^l\end{split}\]- Variables
state – state \(\boldsymbol{\chi}\).
omega – input \(\boldsymbol{\omega}\).
w – noise \(\mathbf{w}\).
dt – integration step \(dt\) (s).
-
classmethod
h(state)[source]¶ Observation function for 1 landmark.
\[h\left(\boldsymbol{\chi}\right) = \mathbf{C}^T \left( \mathbf{p} - \mathbf{p}^l\right) \]- Variables
state – state \(\boldsymbol{\chi}\).
-
classmethod
z(state, y)[source]¶ Augmentation function.
Return a vector of the novel part of the state only.
\[z\left(\boldsymbol{\chi}, \mathbf{y}\right) = \mathbf{C} \mathbf{y} + \mathbf{p}\]- Variables
state – state \(\boldsymbol{\chi}\).
y – measurement \(\mathbf{y}\).
-
classmethod
aug_z(state, y)[source]¶ Augmentation function. Return the augmented state.
\[\boldsymbol{\chi} \leftarrow \left(\boldsymbol{\chi}, z\left(\boldsymbol{\chi}, \mathbf{y}\right) \right) \]- Variables
state – state \(\boldsymbol{\chi}\).
y – measurement \(\mathbf{y}\).
-
classmethod
phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C} \exp\left(\boldsymbol{\xi}^{(0)}\right) \\ \mathbf{p} + \boldsymbol{\xi}^{(1:3)} \\ \mathbf{p}_1^l + \boldsymbol{\xi}^{(3:5)} \\ \vdots \\ \mathbf{p}_L^l + \boldsymbol{\xi}^{(3+2L:5+2L)} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(2) \times \mathbb R^{2(L+1)}\).
Its corresponding inverse operation (for robot state only) is
red_phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
red_phi(state, xi)[source]¶ Retraction (reduced).
The retraction
phi()applied on the robot state only.
-
classmethod
red_phi_inv(state, hat_state)[source]¶ Inverse retraction (reduced).
\[\begin{split}\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \left( \begin{matrix} \log\left(\mathbf{C} \mathbf{\hat{C}}^T\right) \\ \mathbf{p} - \mathbf{\hat{p}} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(2) \times \mathbb R^{2(L+1)}\).
Its corresponding retraction is
red_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
aug_phi(state, xi)[source]¶ Retraction used for augmenting state.
The retraction
phi()applied on the robot state only.
-
classmethod
aug_phi_inv(state, aug_state)[source]¶ Retraction used for augmenting state.
The inverse retraction
phi()applied on the landmark only.
-
classmethod
up_phi(state, xi)[source]¶ Retraction used for updating state and infering Jacobian.
The retraction
phi()applied on the robot state and one landmark only.
-
classmethod
left_phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C} \mathbf{C}_\mathbf{T} \\ \mathbf{p} + \mathbf{C} \mathbf{r}_1 \\ \mathbf{p}_1^l + \mathbf{C} \mathbf{r}_2 \\ \vdots \\ \mathbf{p}_L^l + \mathbf{C} \mathbf{r}_{1+L} \\ \end{matrix} \right)\end{split}\]where
\[\begin{split}\mathbf{T} = \exp\left(\boldsymbol{\xi}\right) = \begin{bmatrix} \mathbf{C}_\mathbf{T} & \mathbf{r}_1 & \cdots & \mathbf{r}_{1+L} \\ \mathbf{0}^T & & \mathbf{I}& \end{bmatrix}\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE_{1+L}(2)\) with left multiplication.
- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
left_red_phi(state, xi)[source]¶ Retraction (reduced).
The retraction
left_phi()applied on the robot state only.
-
classmethod
left_red_phi_inv(state, hat_state)[source]¶ Inverse retraction (reduced).
\[\varphi^{-1}_{\boldsymbol{\hat{\chi}}} \left(\boldsymbol{\chi}\right) = \log\left(\boldsymbol{\chi} \boldsymbol{\hat{\chi}}^{-1}\right)\]The robot state is viewed as a element \(\boldsymbol{\chi} \in SE(2)\).
Its corresponding retraction is
left_red_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
left_aug_phi(state, xi)[source]¶ Retraction used for augmenting state.
The retraction
left_phi()applied on the robot state only.
-
classmethod
left_aug_phi_inv(state, aug_state)[source]¶ Retraction used for augmenting state.
The inverse retraction
left_phi()applied on the landmark only.
-
classmethod
left_up_phi(state, xi)[source]¶ Retraction used for updating state and infering Jacobian.
The retraction
left_phi()applied on the robot state and one landmark only.
-
classmethod
right_phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C}_\mathbf{T} \mathbf{C} \\ \mathbf{C}_\mathbf{T}\mathbf{p} + \mathbf{r}_1 \\ \mathbf{C}_\mathbf{T} \mathbf{p}_1^l + \mathbf{r}_2 \\ \vdots \\ \mathbf{C}_\mathbf{T} \mathbf{p}_L^l + \mathbf{r}_{1+L} \\ \end{matrix} \right)\end{split}\]where
\[\begin{split}\mathbf{T} = \exp\left(\boldsymbol{\xi}\right) = \begin{bmatrix} \mathbf{C}_\mathbf{T} & \mathbf{r}_1 & \cdots & \mathbf{r}_{1+L} \\ \mathbf{0}^T & & \mathbf{I}& \end{bmatrix}\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE_{1+L}(2)\) with right multiplication.
- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
right_red_phi(state, xi)[source]¶ Retraction (reduced).
The retraction
right_phi().
-
classmethod
right_red_phi_inv(state, hat_state)[source]¶ Inverse retraction (reduced).
\[\varphi^{-1}_{\boldsymbol{\hat{\chi}}} \left(\boldsymbol{\chi}\right) = \log\left(\boldsymbol{\hat{\chi}}^{-1} \boldsymbol{\chi}\right)\]The robot state is viewed as a element \(\boldsymbol{\chi} \in SE_{L+1}(2)\).
Its corresponding retraction is
right_red_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
right_aug_phi(state, xi)[source]¶ Retraction used for augmenting state.
The retraction
right_phi()applied on the robot state only.
-
classmethod
right_aug_phi_inv(state, aug_state)[source]¶ Retraction used for augmenting state.
The inverse retraction
right_phi()applied on the landmark only.
-
classmethod
right_up_phi(state, xi)[source]¶ Retraction used for updating state and infering Jacobian.
The retraction
right_phi()applied on the robot state and one landmark only.
IMU-GNSS Fusion on the KITTI Dataset¶
-
class
ukfm.IMUGNSS[source]¶ IMU-GNSS sensor-fusion on the KITTI dataset. The model is the standard 3D kinematics model based on inertial inputs and kinematics equations.
-
g= array([ 0. , 0. , -9.82])¶ gravity vector (m/s^2) \(\mathbf{g}\).
-
class
STATE(Rot, v, p, b_gyro, b_acc)[source]¶ State of the system.
It represents the state of a moving vehicle with IMU biases.
\[ \boldsymbol{\chi} \in \mathcal{M} = \left\{ \begin{matrix} \mathbf{C} \in SO(3), \mathbf{v} \in \mathbb R^3, \mathbf{p} \in \mathbb R^3, \mathbf{b}_g \in \mathbb R^3, \mathbf{b}_a \in \mathbb R^3 \end{matrix} \right\}\]- Variables
Rot – rotation matrix \(\mathbf{C}\).
v – velocity vector \(\mathbf{v}\).
p – position vector \(\mathbf{p}\).
b_gyro – gyro bias \(\mathbf{b}_g\).
b_acc – accelerometer bias \(\mathbf{b}_a\).
-
class
INPUT(gyro, acc)[source]¶ Input of the propagation model.
The input is a measurement from an Inertial Measurement Unit (IMU).
\[\boldsymbol{\omega} \in \mathcal{U} = \left\{ \begin{matrix} \mathbf{u} \in \mathbb R^3, \mathbf{a}_b \in \mathbb R^3 \end{matrix} \right\}\]- Variables
gyro – 3D gyro \(\mathbf{u}\).
acc – 3D accelerometer (measurement in body frame) \(\mathbf{a}_b\).
-
classmethod
f(state, omega, w, dt)[source]¶ Propagation function.
\[\begin{split}\mathbf{C}_{n+1} &= \mathbf{C}_{n} \exp\left(\left(\mathbf{u} - \mathbf{b}_g + \mathbf{w}^{(0:3)} \right) dt\right) \\ \mathbf{v}_{n+1} &= \mathbf{v}_{n} + \mathbf{a} dt, \\ \mathbf{p}_{n+1} &= \mathbf{p}_{n} + \mathbf{v}_{n} dt + \mathbf{a} dt^2/2 \\ \mathbf{b}_{g,n+1} &= \mathbf{b}_{g,n} + \mathbf{w}^{(6:9)}dt \\ \mathbf{b}_{a,n+1} &= \mathbf{b}_{a,n} + \mathbf{w}^{(9:12)} dt \end{split}\]where
\[\mathbf{a} = \mathbf{C}_{n} \left( \mathbf{a}_b -\mathbf{b}_a + \mathbf{w}^{(3:6)} \right) + \mathbf{g}\]Ramdom-walk noises on biases are not added as the Jacobian w.r.t. these noise are trivial. This spares some computations of the UKF.
- Variables
state – state \(\boldsymbol{\chi}\).
omega – input \(\boldsymbol{\omega}\).
w – noise \(\mathbf{w}\).
dt – integration step \(dt\) (s).
-
classmethod
h(state)[source]¶ Observation function.
\[h\left(\boldsymbol{\chi}\right) = \mathbf{p}\]- Variables
state – state \(\boldsymbol{\chi}\).
-
classmethod
phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C} \exp\left(\boldsymbol{\xi}^{(0:3)}\right) \\ \mathbf{v} + \boldsymbol{\xi}^{(3:6)} \\ \mathbf{p} + \boldsymbol{\xi}^{(6:9)} \\ \mathbf{b}_g + \boldsymbol{\xi}^{(9:12)} \\ \mathbf{b}_a + \boldsymbol{\xi}^{(12:15)} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3) \times \mathbb R^{15}\).
Its corresponding inverse operation is
phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\begin{split}\varphi^{-1}_{\boldsymbol{\hat{\chi}}} \left(\boldsymbol{\chi}\right) = \left( \begin{matrix} \log\left(\mathbf{C} \mathbf{\hat{C}}^T \right)\\ \mathbf{v} - \mathbf{\hat{v}} \\ \mathbf{p} - \mathbf{\hat{p}} \\ \mathbf{b}_g - \mathbf{\hat{b}}_g \\ \mathbf{b}_a - \mathbf{\hat{b}}_a \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3) \times \mathbb R^{15}\).
Its corresponding retraction is
phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
up_phi(state, xi)[source]¶ Retraction used for updating state and infering Jacobian.
The retraction
phi()applied on the position state.
-
classmethod
left_phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C} \mathbf{C}_\mathbf{T} \\ \mathbf{v} + \mathbf{C} \mathbf{r_1} \\ \mathbf{p} + \mathbf{C} \mathbf{r_2} \\ \mathbf{b}_g + \boldsymbol{\xi}^{(9:12)} \\ \mathbf{b}_a + \boldsymbol{\xi}^{(12:15)} \end{matrix} \right)\end{split}\]where
\[\begin{split}\mathbf{T} = \exp\left(\boldsymbol{\xi}^{(0:9)}\right) = \begin{bmatrix} \mathbf{C}_\mathbf{T} & \mathbf{r_1} &\mathbf{r}_2 \\ \mathbf{0}^T & & \mathbf{I} \end{bmatrix}\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE_2(3) \times \mathbb{R}^6\) with left multiplication.
Its corresponding inverse operation is
left_phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
left_phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\begin{split}\varphi^{-1}_{\boldsymbol{\hat{\chi}}} \left(\boldsymbol{\chi}\right) = \left( \begin{matrix} \log\left( \boldsymbol{\chi}^{-1} \boldsymbol{\hat{\chi}} \right) \\ \mathbf{b}_g - \mathbf{\hat{b}}_g \\ \mathbf{b}_a - \mathbf{\hat{b}}_a \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE_2(3)\) with left multiplication.
Its corresponding retraction is
left_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
right_phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \mathbf{C}_\mathbf{T} \mathbf{C} \\ \mathbf{C}_\mathbf{T}\mathbf{v} + \mathbf{r_1} \\ \mathbf{C}_\mathbf{T}\mathbf{p} + \mathbf{r_2} \\ \mathbf{b}_g + \boldsymbol{\xi}^{(9:12)} \\ \mathbf{b}_a + \boldsymbol{\xi}^{(12:15)} \end{matrix} \right)\end{split}\]where
\[\begin{split}\mathbf{T} = \exp\left(\boldsymbol{\xi}^{(0:9)}\right) = \begin{bmatrix} \mathbf{C}_\mathbf{T} & \mathbf{r_1} &\mathbf{r}_2 \\ \mathbf{0}^T & & \mathbf{I} \end{bmatrix}\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE_2(3) \times \mathbb{R}^6\) with right multiplication.
Its corresponding inverse operation is
right_phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
right_phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\begin{split}\varphi^{-1}_{\boldsymbol{\hat{\chi}}} \left(\boldsymbol{\chi}\right) = \left( \begin{matrix} \log\left( \boldsymbol{\hat{\chi}}^{-1} \boldsymbol{\chi} \right) \\ \mathbf{b}_g - \mathbf{\hat{b}}_g \\ \mathbf{b}_a - \mathbf{\hat{b}}_a \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SE_2(3) \times \mathbb{R}^6\) with right multiplication.
Its corresponding retraction is
right_phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).
-
classmethod
right_up_phi(state, xi)[source]¶ Retraction used for updating state and infering Jacobian.
The retraction
right_phi()applied on the position state.
-
Spherical Pendulum¶
-
class
ukfm.PENDULUM(T, model_freq)[source]¶ Pendulum example, where the state lives on the 2-sphere. See a text description of the spherical pendulum dynamics in [SE19], Section 7, and [KS19].
- Parameters
T – sequence time (s).
model_freq – model frequency (Hz).
-
g= 9.81¶ gravity constant (m/s^2) \(g\).
-
m= 1.0¶ mass of payload (kg) \(m\).
-
b= 0.0¶ damping \(b\).
-
L= 1.3¶ wire length \(L\).
-
e3= array([ 0, 0, -1])¶ third coordinate vector \(\mathbf{e}^b=-[0,0,1]^T\).
-
H= array([[0., 1., 0.], [0., 0., 1.]])¶ observability matrix \(\mathbf{H}\).
-
class
STATE(Rot, u)[source]¶ State of the system.
It represents the orientation of the wire and its angular velocity.
\[ \boldsymbol{\chi} \in \mathcal{M} = \left\{ \begin{matrix} \mathbf{C} \in SO(3), \mathbf{u} \in \mathbb R^3 \end{matrix} \right\}\]- Variables
Rot – rotation matrix \(\mathbf{C}\).
u – angular velocity vector \(\mathbf{u}\).
-
classmethod
f(state, omega, w, dt)[source]¶ Propagation function.
\[\begin{split}\mathbf{C}_{n+1} &= \mathbf{C}_{n} \exp\left(\left(\mathbf{u} + \mathbf{w}^{(0:3)} \right) dt\right), \\ \mathbf{u}_{n+1} &= \mathbf{u}_{n} + \dot{\mathbf{u}} dt,\end{split}\]where
\[\begin{split}\dot{\mathbf{u}} = \begin{bmatrix} -\omega_y \omega_x\ \\ \omega_x \omega_z \\ 0 \end{bmatrix} + \frac{g}{l} \left(\mathbf{e}^b \right)^\wedge \mathbf{C}^T \mathbf{e}^b + \mathbf{w}^{(3:6)} \end{split}\]- Variables
state – state \(\boldsymbol{\chi}\).
omega – input \(\boldsymbol{\omega}\).
w – noise \(\mathbf{w}\).
dt – integration step \(dt\) (s).
-
classmethod
h(state)[source]¶ Observation function.
\[h\left(\boldsymbol{\chi}\right) = \mathbf{H} \mathbf{x},\]where
\[\begin{split}\mathbf{H}&= \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \\ \mathbf{x} &= L \mathbf{C} \mathbf{e}^b\end{split}\]with \(\mathbf{x}\) the position of the pendulum.
- Variables
state – state \(\boldsymbol{\chi}\).
-
classmethod
phi(state, xi)[source]¶ Retraction.
\[\begin{split}\varphi\left(\boldsymbol{\chi}, \boldsymbol{\xi}\right) = \left( \begin{matrix} \exp\left(\boldsymbol{\xi}^{(0:3)}\right) \mathbf{C} \\ \mathbf{u} + \boldsymbol{\xi}^{(3:6)} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3) \times \mathbb R^3\).
Its corresponding inverse operation is
phi_inv().- Variables
state – state \(\boldsymbol{\chi}\).
xi – state uncertainty \(\boldsymbol{\xi}\).
-
classmethod
phi_inv(state, hat_state)[source]¶ Inverse retraction.
\[\begin{split}\varphi^{-1}_{\boldsymbol{\hat{\chi}}}\left(\boldsymbol{\chi} \right) = \left( \begin{matrix} \log\left(\mathbf{\hat{C}}^T \mathbf{C} \right)\\ \mathbf{u} - \mathbf{\hat{u}} \end{matrix} \right)\end{split}\]The state is viewed as a element \(\boldsymbol{\chi} \in SO(3) \times \mathbb R^3\).
Its corresponding retraction is
phi().- Variables
state – state \(\boldsymbol{\chi}\).
hat_state – noise-free state \(\boldsymbol{\hat{\chi}}\).