Inertial Navigation System
Coordinate Frames ¶
The body frame b is the coordinate frame of the moving IMU. Its origin is located in the center of the accelerometer triad and it is aligned to the casing. All the inertial measurements are resolved in this frame.
The local navigation frame n is a local geographic frame in which we want to navigate. We are interested in the position and orientation of the b-frame with respect to this n-frame .
The inertial frame i is a stationary frame. The IMU measures linear acceleration and angular velocity with respect to this frame. Its origin is located at the center of the earth and its axes are aligned with respect to the stars.
The earth (ECEF) frame e coincides with the i-frame, but rotates with the earth. That is, it has its origin at the center of the earth and axes which are fixed with respect to the earth.
IMU measurements ¶
Angular Velocity ¶
The gyroscope measures the angular velocity of the body frame with respect to the inertial frame, expressed in the body frame denoted by $\omega_{ib}^b$ . $$ \begin{align} \omega_{ib}^b = R^{bn}(\omega_{ie}^n + \omega_{en}^n) + \omega_{nb}^b \end{align} $$
where $R^{bn}$ is a rotation matrix from local navigation frame to body frame. $\omega_{ie}^n$ is the earth rotating rate with respected to inertial frame expressed in local navigation frame, which is about $7.29*10^{−5}\ \text{rad/s}$. $\omega_{en}^n$ is 0 if the local navigation system is defined stationary on the earth.
Linear Acceleration ¶
The accelerometer measures the specific force $a^b$ in the body frame. $$ \begin{align} a_{ii}^b = R^{bn}(a_{ii}^n - g^n) \end{align} $$ where $g$ is the gravity vector expressed in local navigation frame, and $a_{ii}^n$ is the linear acceleration expressed in local navigation frame. $$ \begin{align} a_{ii}^n = R^{ni}a_{ii}^i \end{align} $$
The velocity and acceleration can be obtained by differentiation in a corresponding frame as: $$ \begin{align} \frac{\partial}{\partial{t}}p^n|_n = v_n^n \\ \frac{\partial}{\partial{t}}v^n|_n = a_{nn}^n \\ \end{align} $$
Concept Review
Suppose we have a frame v is only rotating with respect to frame u, then in general we have:
Suppose we have a frame v is rotating and moving with respect to frame u, then in general we have:
where $\frac{\partial}{\partial{t}}r_{uv}^u|_u$ is the moving speed, $\omega_{uv}^{v}$ is angular velocity expressed in the moving frame $v$.
So suppose b frame, e.g. earth frame, is rotating with respect g frame, e.g. inertial frame, but without any translation, given $v = [\omega]_{\times}r $, then for a point $p$ in frame b, we have its velocity and acceleration expressed in frame g as: $$ \begin{align} v^g_{g} &= R^{gb}(v^b_{b} +[\omega_{gb}^b]_{\times}r_{o^{b}p}^b) \\ a^g_{gg} &= R^{gb}(a^b_{bb} + \dot{\omega}_{gb}^b + 2[\omega_{gb}^b]_{\times}v^b_{b} + [\omega_{gb}^b]_{\times}([\omega_{gb}^b]_{\times}r_{o^{b}p}^b)) \end{align} $$ where $\dot{\omega}_{gb}^b$ is angular acceleration, $2[\omega_{gb}^b]_{\times}v^b_{b}$ is Coriolis acceleration, $[\omega_{gb}^b]_{\times}([\omega_{gb}^b]_{\times}r_{o^{b}p}^b$ is Centripetal acceleration.
Since earth frame is only rotating around the inertial frame, so we can get: $$ \begin{align} v_{i}^i &= R^{ie}(v_e^e + [\omega_{ie}^e]_{\times}r_{o^{e}p}^e)\\ &=v^i_e + [\omega_{ie}^i]_{\times}r_{o^{e}p}^i\\ a_{ii}^i &= R^{ie}(a_{ee}^e + + \dot{\omega}_{ie}^e + 2[\omega_{ie}^e]_{\times}v^e_{e} + [\omega_{ie}^e]_{\times}([\omega_{ie}^e]_{\times}r_{o^{e}p}^e))\\ &= a^i_{ee} + 0 + 2[\omega_{ie}^i]_{\times}v_e^i + [\omega_{ie}^i]_{\times}([\omega_{ie}^i]_{\times}r_{o^{e}p}^i) \end{align} $$ where we assume the angular velocity of the earth is constant, i.e. $\dot{\omega}_{ie}^e = 0$
Similarly, we can get the velocity and acceleration relation between earth frame and navigation frame where we assume navigation frame is stationary with respect to the earth frame, i.e. $[\omega_{en}^n] = 0$: $$ \begin{align} v_{e}^e &= R^{en}(v_n^n + [\omega_{en}^n]_{\times}r_{o^{n}p}^n)\\ &=v^e_n \\ v_e^n &= R^{ne}v_n^e = v_n^n \\ a_{ee}^e &= R^{en}(a_{nn}^n + + \dot{\omega}_{en}^e + 2[\omega_{en}^n]_{\times}v^n_{n} + [\omega_{en}^n]_{\times}([\omega_{en}^n]_{\times}r_{o^{n}p}^n))\\ &= a^e_{nn} \\ a_{ee}^n &= R^{ne}a_{nn}^e = a_{nn}^n \end{align} $$
Because we have: $$ \begin{align} v_e^n &= R^{ne}v_n^e = v_n^n \\ a_{ee}^n &= R^{ne}a_{nn}^e = a_{nn}^n \end{align} $$
Finally, we get an important equation: $$ \begin{align} a_{ii}^n &= R^{ni}a_{ii}^i \\ & = R^{ni}(a^i_{ee} + 0 + 2[\omega_{ie}^i]_{\times}v_e^i + [\omega_{ie}^i]_{\times}([\omega_{ie}^i]_{\times}r_{o^{e}p}^i)) \\ & = a^n_{ee} + 2[\omega_{ie}^n]_{\times}v_e^n + [\omega_{ie}^n]_{\times}([\omega_{ie}^n]_{\times}r_{o^{e}p}^n)\\ & = a^n_{nn} + 2[\omega_{ie}^n]_{\times}v_n^n + [\omega_{ie}^n]_{\times}([\omega_{ie}^n]_{\times}r_{o^{e}p}^n) \end{align} $$
Recall that we have the measurement model as: $$ \begin{align} a^b = R^{bn}(a_{ii}^n - g^n) \end{align} $$ So we can get a correlation between the measurement $a^b$ and the acceleration state $a^n_{nn}$ in local navigation frame.
Normally, because Centrifugal acceleration is small in the order of $10^{−2}\ m/s^2$ given earth rotation is about $7.29*10^{−5}\ \text{rad}/s$. For Coriolis acceleration, when car travel 80 miles per hour, it is in the order of $10^{−3}\ m/s^2$. So we can normally use $a_{ii}^n = a_{nn}^n$
So we have the measurement model as: Recall that we have the measurement model as: $$ \begin{align} a^b = R^{bn}(a_{nn}^n - g^n) \end{align} $$
Measurement Model ¶
Gyroscope measurement models ¶
The gyroscope measures the angular velocity $\omega^{b}_{ib} $ at each time instance $t$, but its measurements are corrupted by a slowly time-varying bias $\delta_{\omega,t}$, and noise $e_{\omega,t}$: $$ \begin{align} y_{\omega,t} = \omega^{b}_{ib,t} + \delta_{\omega,t}^b + e_{\omega,t}^b \end{align} $$
where $e_{\omega,t}^b \sim \mathcal{N}(0, \Sigma_{\omega})$. If the gyroscope is properly calibrated, we can think that measurements in three gyroscope axes are independent.
The gyroscope bias $\delta_{\omega,t}^b$ is slowly time-varying: $$ \begin{align} \delta_{\omega,t+1}^b = \delta_{\omega,t}^b + e_{\delta_{\omega},t}^b \end{align} $$ where $e_{\delta_{\omega},t}^b \sim \mathcal{N}(0, \Sigma_{\delta_{\omega}})$
From previous section we have $\omega_{ib}^b = R^{bn}(\omega_{ie}^n + \omega_{en}^n) + \omega_{nb}^b$. If we assume local navigation frame is stationary and neglect earth rotation, we can get: $$ \begin{align} \omega_{ib}^b = \omega_{nb}^b \end{align} $$
Then we can get gyroscope measurement model as: $$ \begin{align} y_{\omega,t} = \omega^{b}_{nb,t} + \delta_{\omega,t}^b + e_{\omega,t}^b \end{align} $$
Accelerometer measurement models ¶
The accelerometer measures the acceleration $a^{b}_{ii} $ at each time instance $t$, but its measurements are corrupted by a slowly time-varying bias $\delta_{a,t}$, and noise $e_{a,t}$: $$ \begin{align} y_{a,t} = a^{b}_{ii,t} + \delta_{a,t}^b + e_{a,t}^b \end{align} $$
where $e_{a,t}^b \sim \mathcal{N}(0, \Sigma_{a})$. If the gyroscope is properly calibrated, we can think that measurements in three gyroscope axes are independent.
The accelerometer bias $\delta_{a,t}^b$ is slowly time-varying: $$ \begin{align} \delta_{a,t+1}^b = \delta_{a,t}^b + e_{\delta_{a},t}^b \end{align} $$ where $e_{\delta_{a},t}^b \sim \mathcal{N}(0, \Sigma_{\delta_{a}})$
As we discussed in the previous section that we have $a_{ii}^b = R^{bn}(a_{ii}^n - g^n)$, then $$ \begin{align} y_{a,t} = a^{b}_{ii,t} + \delta_{a,t}^b + e_{a,t}^b \end{align} $$
If we neglect Centrifugal acceleration and Coriolis acceleration, we have $a_{ii}^n = a_{nn}^n$, the we can get accelerometer measurement model as: $$ \begin{align} y_{a,t} = R^{bn}(a_{nn}^n - g^n) + \delta_{a,t}^b + e_{a,t}^b \end{align} $$
State and Dynamics ¶
Recall that we have two measurement models: $$ \begin{align} y_{\omega,t} &= \omega^{b}_{nb,t} + \delta_{\omega,t}^b + e_{\omega,t}^b \\ y_{a,t} &= (q^{nb})^{-1} \odot (a_{nn}^n - g^n) + \delta_{a,t}^b + e_{a,t}^b \end{align} $$
Constant Acceleration Model ¶
We can have a state vector as $x_t = (p_t^n, v_{n,t}^n, a_{nn,t}^n, \delta_{a,t}^b, q^{nb}_t, \omega^b_{nb, t}, \delta_{\omega,t}^b )$.
Assume that acceleration is constant and angular velocity is constant, we can get: $$ \begin{align} p_{t+1}^n &= p_t^n + T v_{n,t}^n + \frac{T^2}{2}a_{nn,t}^n \\ v_{n,t+1}^n &= v_{n,t}^n + T a_{nn,t}^n \\ a_{nn,t+1}^n &= a_{nn,t} + e_{a,t} \\ \delta_{a,t+1}^b &= \delta_{a,t}^b + e_{\delta_{a},t}^b \\ q^{nb}_{t+1} &= q^{nb}_t \odot Exp_q(\frac{T}{2}\omega^b_{nb, t}) = R^{nb}_t Exp_R(T\omega^b_{nb, t})\\ \omega^b_{nb, t+1} &= \omega^b_{nb, t} + e_{\omega,t} \\ \delta_{\omega,t+1}^b &= \delta_{\omega,t}^b + e_{\delta_{\omega},t}^b \end{align} $$
where $e_{a,t} \sim \mathcal{N}(0, \Sigma_{a})$, $e_{\omega,t} \sim \mathcal{N}(0, \Sigma_{\omega})$. $Exp()$ is an exponential operation as: $$ \begin{align} Exp_q(\eta) &= (cos(\|\eta\|), \frac{\eta}{\|\eta\|}sin(\|\eta\|)))^T \\ Exp_R(\eta) &= \mathcal{I}_3 + sin(\|\eta\|)[\frac{\eta}{\|\eta\|}]_{\times} + (1-cos(\|\eta\|)[\frac{\eta}{\|\eta\|}]_{\times}^2 \end{align} $$ Note for an angle axis vector described as $(\alpha, \textbf{n})$, it is equal to $Exp_q(\frac{\alpha}{2}\textbf{n})$ and $Exp_R(\alpha\textbf{n})$.
Inertial Measurements Driven Model ¶
Instead of modeling $a_{nn,t}^n$ and $\omega^b_{nb, t}$ in the state vector, we could use IMU's measurements $y_{\omega,t}$ and $y_{a,t}$ to update the state. Hence, the state vector will be $(p_t^n, v_{n,t}^n, \delta_{a,t}^b, q^{nb}_t, \delta_{\omega,t}^b )$
Note:
- $(q_t^{nb}(y_{a,t}-\delta_{a,t}^b-e_{a,t}^b)+g^n)$ could be simplified as $(q_t^{nb}(y_{a,t}-\delta_{a,t}^b)-e_{a,t}^b+g^n)$ if we consider $e_{a,t}$ have same noise for three axes as $\sim \mathcal{N}(0, \sigma_a^2\mathcal{I}_3)$.
- This numeric integration of the velocity and position assumes a constant orientation $q^{nb}_t$ for the time of integration between two measurements.
State Estimation ¶
Factor Graph based Optimization ¶
Firstly we will define the cost function to minimize along with the variables we want to optimize over. For position and orientation estimation of INS, if we use state vector as $(p_t^n, v_{n,t}^n, \delta_{a,t}^b, q^{nb}_t, \delta_{\omega,t}^b )$, we can define the optimization problem as:
$$ \begin{align} \hat{x}_{0:N} = \underset{x_{0:N}}{\operatorname{argmin}}& \|e_{p,0}\|_{\Sigma^{-1}_{p^n_0}} + \|e_{v,0}\|_{\Sigma^{-1}_{v^n_0}} + \|e_{q,0}\|_{\Sigma^{-1}_{q^{nb}_0}} \\ +&\ \|e_{p,a,t}\|_{\Sigma^{-1}_{e_a}} + \|e_{v,a,t}\|_{\Sigma^{-1}_{e_a}} + \|e_{\omega,t}\|_{\Sigma^{-1}_{e_{\omega}}} \\ +&\ \|e_{\delta_{\omega},t}\|_{\Sigma^{-1}_{\delta_{\omega}}} + \|e_{\delta_{a},t}\|_{\Sigma^{-1}_{\delta_{a}}} \\ +&\ \|e_{m,t}\|_{\Sigma^{-1}_{e_m}} \end{align} $$Note: we could optimize over state deviation, e.g. orientation deviation $\eta^{nb}$ instead of orientation $q^{nb}$.
Prior errors: $$ \begin{align} e_{p,0} &= p^n_0 - y_{p,0} \\ e_{v,0} &= v^n_0 - y_{v,0} \\ e_{q,0} &= Log(q^{nb}_0 \odot y_{q, 0}) \end{align} $$
IMU dynamics errors: $$ \begin{align} e_{p,a,t} &= \frac{2}{T^2}(p^n_{t+1} - p^n_{t} - T v^n_{n,t}) - g^n - q^{bn} \odot(y_{a,t}-\delta_{a,t}^b) \\ e_{v,a,t} &= \frac{1}{T}(v^n_{t+1}-v^n_{t}) - g^n - q^{bn} \odot(y_{a,t}-\delta_{a,t}^b) \\ e_{\omega,t} &= \frac{2}{T}Log(q_t^{bn} \odot q_{t+1}^{nb}) - y_{\omega,t} + \delta_{\omega,t} \end{align} $$
Bias errors: $$ \begin{align} e_{\delta_{\omega},t} &= \delta_{a,t+1}^b - \delta_{a,t}^b\\ e_{\delta_{a},t} &= \delta_{\omega,t+1}^b - \delta_{\omega,t}^b \end{align} $$
Measurement errors:
- Position measurements
- Velocity measurements from wheel-encoder
- Zero angular velocity $$ \begin{align} e_{m,t} &= Log(q_t^{bn} \odot q_{t+1}^{nb}) \end{align} $$
- Zero linear velocity $$ \begin{align} e_{m,t} &= v_t^n \end{align} $$
- Zero acceleration $$ \begin{align} e_{m,t} &= v_{t+1}^n - v_t^n \end{align} $$
I think a challenge to use IMU measurements driven model is that additional measurements might arrive at different freuqency than IMU measurements. We could interpolate the measurements for batch optimization since we have complete information for the whole time. This assumes that we have a state node in our factor graph when every IMU measurement comes in. This will leads to fast growth number of variables in the optimization.
IMU Noise Model ¶
- Noise Density (ND) : It provides the noise divided by the square root of the sampling rate. The noise density for a gyroscope can be represented as $(^{\circ}/s)/\sqrt{Hz}$ or $(^{\circ}/hr)/\sqrt{Hz}$.By multiplying the noise density (ND) by the square root of the sampling rate (SR), the noise standard deviation $\sigma$ at that rate can be recovered $\sigma = ND * \sqrt{SR}$. When ND is specified as Power Spectral Density (PSD) , it is essentially $PSD = (ND)^2$.
- Random Walk (RW) : The specification for random walk is typically given in units of $^{\circ}/\sqrt{s}$ for gyroscopes, and $(m/s)/\sqrt{s}$ for accelerometers. By multiplying the random walk by the square root of time, the standard deviation of the drift due to noise can be recovered $\sigma = \text{RW}\sqrt{t}$. For unit conversion, we can use $60\sqrt{s} = \sqrt{hr}$, $ 1 \text{milli-g} \approx 0.01 m/s^2$.
- Bias : The bias is a constant offset of the output value from the input value. The in-run bias stability, or often called the bias instability, is a measure of how the bias will drift during operation over time at a constant temperature. This parameter also represents the best possible accuracy with which a sensor's bias can be estimated. Due to this, in-run bias stability is generally the most critical specification as it gives a floor to how accurate a bias can be measured.
- Scale Factor : Scale factor is a multiplier on a signal that is comprised of a ratio of the output to the input over the measurement range. This factor typically varies over temperature and must be calibrated for over the operational temperature range.
-
Summaries:
- $\sigma_a$ and $\sigma_{\omega}$ can be obtained by $\sigma_{discrete} = ND * \sqrt{SR} = RW * \sqrt{SR}$
- Based on Kalibr, $\sigma_{\delta_a}$ and $\sigma_{\delta_{\omega}}$ can be obtained by $\sigma_{discrete} = \sigma / \sqrt{SR}$
- Based on Kalibr, we can get continuous noise values based on Allan Variance plot.
Reference:
- https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
- https://github.com/rpng/kalibr_allan
- https://www.mathworks.com/help/fusion/ug/inertial-sensor-noise-analysis-using-allan-variance.html
- https://www.vectornav.com/resources/inertial-navigation-primer/specifications--and--error-budgets/specs-imuspecs
- https://www.vectornav.com/resources/inertial-navigation-primer/examples/noise
- https://telesens.co/wp-content/uploads/2017/05/AllanVariance5087-1.pdf
- https://mwrona.com/posts/gyro-noise-analysis/
- https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
# Allan Plot Sample Code
# From blog post: https://mwrona.com/posts/gyro-noise-analysis/
import numpy as np
import matplotlib.pyplot as plt
def AllanDeviation(dataArr, fs, maxNumM=100):
"""Compute the Allan deviation (sigma) of time-series data.
Algorithm obtained from Mathworks:
https://www.mathworks.com/help/fusion/ug/inertial-sensor-noise-analysis-using-allan-variance.html
Args
----
dataArr (numpy.ndarray): 1D data array
fs (int, float): Data sample frequency in Hz
maxNumM (int): Number of output points
Returns
-------
(taus, allanDev): Tuple of results
taus (numpy.ndarray): Array of tau values
allanDev (numpy.ndarray): Array of computed Allan deviations
"""
ts = 1.0 / fs
N = len(dataArr)
Mmax = 2**np.floor(np.log2(N / 2))
M = np.logspace(np.log10(1), np.log10(Mmax), num=maxNumM)
M = np.ceil(M) # Round up to integer
M = np.unique(M) # Remove duplicates
taus = M * ts # Compute 'cluster durations' tau
# Compute Allan variance
allanVar = np.zeros(len(M))
for i, mi in enumerate(M):
twoMi = int(2 * mi)
mi = int(mi)
allanVar[i] = np.sum(
(dataArr[twoMi:N] - (2.0 * dataArr[mi:N-mi]) + dataArr[0:N-twoMi])**2
)
allanVar /= (2.0 * taus**2) * (N - (2.0 * M))
return (taus, np.sqrt(allanVar)) # Return deviation (dev = sqrt(var))
# Config. params
DATA_FILE = 'gyro-data.csv' # CSV data file "gx,gy,gz"
fs = 100 # Sample rate [Hz]
# Load CSV into np array
dataArr = np.genfromtxt(DATA_FILE, delimiter=',')
ts = 1.0 / fs
# Separate into arrays
gx = dataArr[:, 0] * (180.0 / np.pi) # [deg/s]
gy = dataArr[:, 1] * (180.0 / np.pi)
gz = dataArr[:, 2] * (180.0 / np.pi)
# Calculate gyro angles
thetax = np.cumsum(gx) * ts # [deg]
thetay = np.cumsum(gy) * ts
thetaz = np.cumsum(gz) * ts
# Compute Allan deviations
(taux, adx) = AllanDeviation(thetax, fs, maxNumM=200)
(tauy, ady) = AllanDeviation(thetay, fs, maxNumM=200)
(tauz, adz) = AllanDeviation(thetaz, fs, maxNumM=200)
# Plot data on log-scale
plt.figure()
plt.title('Gyro Allan Deviations')
plt.plot(taux, adx, label='gx')
plt.plot(tauy, ady, label='gy')
plt.plot(tauz, adz, label='gz')
plt.xlabel(r'$\tau$ [sec]')
plt.ylabel('Deviation [deg/sec]')
plt.grid(True, which="both", ls="-", color='0.65')
plt.legend()
plt.xscale('log')
plt.yscale('log')
plt.show()
#
IMU Preintegration ¶
In short, we want to generate IMU factors between keyframes, which can be pre-computed just based on IMU measurements and time interval between keyframes so that they are not required to be recomputed after updating linearization points of state variables. This requires IMU and keyframe timestamps are synchronized .
1) Preintegrated IMU Measurements with assumption that bias is constant . ¶
Suppose we have state $x_i$ at timestamp $t_i$, then $$ \begin{align} \Delta R_{ij} &= R_i^T R_{j} \\ &= \prod_{k=i}^{j-1} Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b - e_{\omega,k}^b))\\ \Delta v_{ij} &= R_i^T(v_j-v_i-(j-i)Tg) \\ &= \sum_{k=i}^{j-1}[T\Delta R_{ik}(y_{a,k}-\delta_{a,k}^b-e_{a,k}^b)]\\ \Delta p_{ij} &= R_i^T(p_j - p_i - (j-i)Tv_i -\frac{1}{2}(j-i)^2T^2g) \\ &= \sum_{k=i}^{j-1}[T\Delta v_{ik} + \frac{1}{2}\Delta R_{ik}(j-i)^2T^2(y_{a,k}-\delta_{a,k}^b-e_{a,k}^b)] \end{align} $$ The equation above clearly describe the relations between state variables and measurements. However, in order to be able to fit into a MAP estimation simply, we need it be in form as $y = f(x) + e$.
Given $Exp(\phi + \delta\phi) \approx\ Exp(\phi)Exp(J_r(\phi)\delta\phi)$ and assume $e_{\omega,k}^b$ is very small, we can have following equations with $\phi_k = T(y_{\omega, k} - \delta_{\omega,k}^b)$ and $\delta \phi = T(-e_{\omega,k}^b)$ $$ \begin{align} \Delta R_{ij} &= \prod_{k=i}^{j-1} Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b - e_{\omega,k}^b)) \\ &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b))Exp_{R}(J_{r_k}T(-e_{\omega,k}^b)) \end{align} $$
Then given $Exp(\phi) R = R Exp(R^T \phi)$, we can have following equations with $\Delta R_k = Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b))$ $$ \begin{align} \Delta R_{ij} &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b))Exp_{R}(J_{r_k}T(-e_{\omega,k}^b)) \\ &= \Delta R_i Exp_{R}(J_{r_i}T(-e_{\omega,i}^b)) \Delta R_{i+1} Exp_{R}(J_{r_{i+1}}T(-e_{\omega,{i+1}}^b))\Delta R_{i+2} \cdots \\ &= \Delta R_i \{Exp_{R}(J_{r_i}T(-e_{\omega,i}^b)) [\Delta R_{i+1}\Delta R_{i+2}]\} \{Exp_{R}(\Delta R_{i+2}^T J_{r_{i+1}}T(-e_{\omega,{i+1}}^b))\} \cdots \\ &= \Delta R_i\Delta R_{i+1}\Delta R_{i+2} \{Exp_{R}((\Delta R_{i+1}\Delta R_{i+2})^TJ_{r_i}T(-e_{\omega,i}^b))\} \{Exp_{R}(\Delta R_{i+2}^T J_{r_{i+1}}T(-e_{\omega,{i+1}}^b))\} \cdots \\ &= \Delta {\tilde{R}_{ij}}\prod_{k=i}^{j-1}[Exp_R(\Delta {\tilde{R}_{k+1,j}^T}J_{r_k}T(-e_{\omega,k}^b))] \\ &= \Delta {\tilde{R}_{ij}}Exp_R(-\delta \phi_{ij}) \end{align} $$ where $\Delta {\tilde{R}_{ij}} = \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b))$
Similarly, we can get: $$ \begin{align} \Delta v_{ij} &= \sum_{k=i}^{j-1}[T\Delta R_{ik}(y_{a,k}-\delta_{a,k}^b-e_{a,k}^b)]\\ &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}Exp_R(-\delta \phi_{ik})(y_{a,k}-\delta_{a,k}^b-e_{a,k}^b)] \end{align} $$ With first-order approximation $Exp_R(\delta \phi) = \mathcal{I} + [\delta \phi]_{\times}$, we can get: $$ \begin{align} \Delta v_{ij} &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}Exp_R(-\delta \phi_{ik})(y_{a,k}-\delta_{a,k}^b-e_{a,k}^b)]\\ &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(\mathcal{I}-[\delta \phi_{ik}]_\times)(y_{a,k}-\delta_{a,k}^b-e_{a,k}^b)] \\ &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(\mathcal{I}-[\delta \phi_{ik}]_\times)(y_{a,k}-\delta_{a,k}^b) - T\Delta {\tilde{R}_{ik}}e_{a,k}^b] \\ \end{align} $$ Note that we omit a higher order noise as $T\Delta {\tilde{R}_{ik}}\delta \phi_{ik}e_{a,k}^b$ in the third line of the equations.
Then with $[a]_\times b = -b_\times a$ $$ \begin{align} \Delta v_{ij} &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(\mathcal{I}-[\delta \phi_{ik}]_\times)(y_{a,k}-\delta_{a,k}^b) - T\Delta {\tilde{R}_{ik}}e_{a,k}^b] \\ &= \Delta{\tilde{v_{ij}}} - \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(e_{a,k}^b + [\delta \phi_{ik}]_\times(y_{a,k}-\delta_{a,k}^b))] \\ &= \Delta{\tilde{v_{ij}}} - \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(e_{a,k}^b - [y_{a,k}-\delta_{a,k}^b]_\times(\delta \phi_{ik}))]\\ &= \Delta{\tilde{v_{ij}}} - \delta v_{ij} \end{align} $$ where $\Delta{\tilde{v_{ij}}} = \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)]$
where $\Delta{\tilde{p}_{ij}} = \sum_{k=i}^{j-1}[T\Delta{\tilde{v_{ij}}}-\frac{1}{2}T^2 {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)]$
2) Noise propagation ¶
Now we get noise term $\delta p_{ij}, \delta v_{ij}, \delta \phi_{ij}$, which also follows zero mean Gaussian distribution. How can we get find correct covariance for them?
Given $Log(Exp(\phi)Exp(\delta\phi)) \approx\ \phi + J_r^{-1}(\phi)\delta\phi$, we have $Log(Exp(\delta \phi_1)Exp(\delta\phi_2)) \approx\ \delta\phi_1 + \delta\phi_2$. Then we have: $$ \begin{align} \delta \phi_{ij} &= -Log(\prod_{k=i}^{j-1}[Exp_R(\Delta {\tilde{R}_{k+1,j}^T}J_{r_k}T(-e_{\omega,k}^b))])\\ & \approx \sum_{k=i}^{j-1} \Delta \tilde{R}_{k+1,j}^T J_{r_k}Te_{\omega,k}^b \end{align} $$ Then we can use linear propagation as $$ \begin{align} \Sigma_{\delta \phi_{ij}} & \approx \sum_{k=i}^{j-1} (\Delta \tilde{R}_{k+1,j}^T J_{r_k}T)\Sigma_{e_{\omega,k}^b}(\Delta \tilde{R}_{k+1,j}^T J_{r_k}T)^T \end{align} $$ Where $\sigma_{\omega}$ can be obtained by $\sigma_{discrete} = ND * \sqrt{SR} = RW * \sqrt{SR}$.
Similarly, noise propagation for velocity and position are also just linear propagation. $$ \begin{align} \delta v_{ij} &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}e_{a,k}^b - T\Delta {\tilde{R}_{ik}}[y_{a,k}-\delta_{a,k}^b]_\times(\delta \phi_{ik})] \end{align} $$
3) Incorporating non-constant bias ¶
From the previous section, we know all pre-integrated measurements depend on bias estimation $\delta_{a,i}$ and $\delta_{\omega,i}$, where we assume constant bias between frames, aka $\delta_{i} == \delta_{j}$. So this means that if the bias estimation changes, we need to re-calculate all measurements which is exactly what we want to avoid.
The solution is to use Taylor expansion $f(x) \approx f(a) + J_x|_{x=a}(x-a)$. $$ \begin{align} \Delta {\tilde{R}_{ij}} &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b)) \\ &= f_{R_{ij}}(\delta_{\omega,i}^b) \\ &= f_{R_{ij}}(\bar{\delta}_{\omega,i}^b + \Delta \delta_{\omega,i}^b) \\ &\approx f_{R_{ij}}(\bar{\delta}_{\omega,i}^b) + J^{R_{ij}}_{\delta_{\omega,i}}|_{\delta_{\omega,i} = \bar{\delta}_{\omega,i}^b} \Delta \delta_{\omega,i}^b \\ \end{align} $$
Calculating the Jacobian matrix $J^{R_{ij}}_{\delta_{\omega,i}}|_{\delta_{\omega,i}= \bar{\delta}_{\omega,i}^b}$ is not trivial. Next, we will figure it out.
$$ \begin{align} \Delta {\tilde{R}_{ij}} &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \delta_{\omega,k}^b)) \\ &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \bar{\delta}_{\omega,i}^b) - T\Delta \delta_{\omega,i}^b) \end{align} $$Based on $Exp(\phi + \delta\phi) \approx Exp(\phi)Exp(J_r(\phi)\delta\phi)$, $\phi = T(y_{\omega, k} - \bar{\delta}_{\omega,i}^b)$ and $\delta \phi = - T\Delta \delta_{\omega,i}^b$, $$ \begin{align} \Delta {\tilde{R}_{ij}} &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \bar{\delta}_{\omega,i}^b) - T\Delta \delta_{\omega,i}^b) \\ &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \bar{\delta}_{\omega,i}^b)\prod_{k=i}^{j-1}Exp_{R}(-J_r^k \Delta \delta_{\omega,i}^b) \end{align} $$
Based on similar derivation for $\Delta R_{ij}$, we can get $$ \begin{align} \Delta {\tilde{R}_{ij}} &= \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \bar{\delta}_{\omega,i}^b)\prod_{k=i}^{j-1}Exp_{R}(-J_{r_k} \Delta \delta_{\omega,i}^b)\\ &= f_{R_{ij}}(\bar{\delta}_{\omega,i}^b)\prod_{k=i}^{j-1}[Exp_R(f_{R_{k+1,j}}(\bar{\delta}_{\omega,i}^b)^T J_{r_k}T(-\Delta \delta_{\omega,i}^b))] \\ \end{align} $$ Then based on $Exp(\phi + J_r^{-1}(\phi)\delta\phi) \approx Exp(\phi)Exp(J_r(\phi)J_r^{-1}(\phi)\delta\phi)$ and assume $f_{R_{_{k+1,j}}}(\bar{\delta}_{\omega,i}^b)^T J_{r_k}T(-\Delta \delta_{\omega,i}^b)$ is also suffient small, we can have: $$ \begin{align} \Delta {\tilde{R}_{ij}} &= f_{R_{ij}}(\bar{\delta}_{\omega,i}^b)\prod_{k=i}^{j-1}[Exp_R(f_{R_{k+1,j}}(\bar{\delta}_{\omega,i}^b)^T J_{r_k}T(-\Delta \delta_{\omega,i}^b))] \\ &= f_{R_{ij}}(\bar{\delta}_{\omega,i}^b)Exp_R(\sum_{k=i}^{j-1}[f_{R_{k+1,j}}(\bar{\delta}_{\omega,i}^b)^T J_{r_k}T(-\Delta \delta_{\omega,i}^b)]) \\ &= f_{R_{ij}}(\bar{\delta}_{\omega,i}^b)Exp_R(\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ij}}}\Delta \delta_{\omega,i}^b) \end{align} $$ where $\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ij}}} = -\sum_{k=i}^{j-1}[f_{R_{k+1,j}}(\bar{\delta}_{\omega,i}^b)^T J_{r_k}T]$
Similarly for speed residual, we can have: $$ \begin{align} \Delta{\tilde{v}_{ij}} &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)]\\ &= f_{v_{ij}}(\delta_{a,i}^b, \delta_{\omega,i}^b) \\ &= f_{v_{ij}}(\bar{\delta}_{a,i}^b+\Delta \delta_{a,i}^b, \bar{\delta}_{\omega,i}^b + \Delta \delta_{\omega,i}^b) \\ &\approx f_{v_{ij}}(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + J^v_{\delta_{a,i}}|_{\delta_{a,i}^b=\bar{\delta}_{a,i}^b} \Delta \delta_{a,i}^b + J^v_{\delta_{\omega,i}}|_{\delta_{\omega,i} = \bar{\delta}_{\omega,i}^b} \Delta \delta_{\omega,i}^b \end{align} $$
$$ \begin{align} \Delta{\tilde{v}_{ij}} &= \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)]\\ &= \sum_{k=i}^{j-1}[T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)Exp_R(\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b))(y_{a,k}-\bar{\delta}_{a,i}^b - \Delta \delta_{a,i}^b)] \\ &\approx \sum_{k=i}^{j-1}[T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)(\mathcal{I} + [\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b)]_{\times})(y_{a,k}-\bar{\delta}_{a,i}^b - \Delta \delta_{a,i}^b)] \\ &= f_{v_{ij}}(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) - \sum_{k=i}^{j-1}[T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)\Delta \delta_{a,i}^b - T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)[\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b]_{\times}(y_{a,k}-\bar{\delta}_{a,i}^b)] + o(\text{high order term})\\ &= f_{v_{ij}}(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) - \sum_{k=i}^{j-1}[T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)\Delta \delta_{a,i}^b + T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)[y_{a,k}-\bar{\delta}_{a,i}^b]_{\times}\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b]\\ &= f_{v_{ij}}(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + \mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ij}}}\Delta \delta_{a,i}^b + \mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ij}}}\Delta \delta_{\omega,i}^b \end{align} $$Similarly for position residual, we can have: $$ \begin{align} \Delta{\tilde{p}_{ij}} &= \sum_{k=i}^{j-1}[T\Delta{\tilde{v_{ij}}}-\frac{1}{2}T^2 {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)] \\ &= f_p(\delta_{a,i}^b, \delta_{\omega,i}^b) \\ &= f_p(\bar{\delta}_{a,i}^b+\Delta \delta_{a,i}^b, \bar{\delta}_{\omega,i}^b + \Delta \delta_{\omega,i}^b) \\ &\approx f_p(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + J^p_{\delta_{a,i}}|_{\delta_{a,i}^b=\bar{\delta}_{a,i}^b} \Delta \delta_{a,i}^b + J^p_{\delta_{\omega,i}}|_{\delta_{\omega,i} = \bar{\delta}_{\omega,i}^b} \Delta \delta_{\omega,i}^b \end{align} $$
$$ \begin{align} \Delta{\tilde{p}_{ij}} &= \sum_{k=i}^{j-1}[T\Delta{\tilde{v_{ij}}}-\frac{1}{2}T^2 {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)] \\ &= \sum_{k=i}^{j-1}[T f_{v_{ik}}(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + T\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ik}}}\Delta \delta_{a,i}^b + T\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ik}}}\Delta \delta_{\omega,i}^b -\frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)Exp_R(\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b))(y_{a,k}-\bar{\delta}_{a,i}^b - \Delta \delta_{a,i}^b)] \\ &\approx \sum_{k=i}^{j-1}[T f_{v_{ik}}(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + T\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ik}}}\Delta \delta_{a,i}^b + T\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ik}}}\Delta \delta_{\omega,i}^b -\frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)(\mathcal{I} + [\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b)]_{\times})(y_{a,k}-\bar{\delta}_{a,i}^b - \Delta \delta_{a,i}^b)] \\ &= f_p(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + \sum_{k=i}^{j-1}[T\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ik}}}\Delta \delta_{a,i}^b + T\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ik}}}\Delta \delta_{\omega,i}^b -\frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)\Delta \delta_{a,i}^b - \frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)[\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b]_{\times}(y_{a,k}-\bar{\delta}_{a,i}^b)] + o(\text{high order term}) \\ &= f_p(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + \sum_{k=i}^{j-1}[T\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ik}}}\Delta \delta_{a,i}^b + T\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ik}}}\Delta \delta_{\omega,i}^b -\frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)\Delta \delta_{a,i}^b - \frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)[y_{a,k}-\bar{\delta}_{a,i}^b]_{\times}\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}\Delta \delta_{\omega,i}^b] \\ &= f_p(\bar{\delta}_{a,i}^b, \bar{\delta}_{\omega,i}^b) + \mathcal{J}_{\delta_{a,i}^b}^{f_{p_{ij}}}\Delta \delta_{a,i}^b + \mathcal{J}_{\delta_{\omega,,i}^b}^{f_{p_{ij}}}\Delta \delta_{\omega,i}^b \end{align} $$4) Summary ¶
Base on the derivations in previous three sections, we can finally find the residual functions as: $$ \begin{align} r_{\Delta R_{ij}} &= Log(\Delta {\tilde{R}_{ij}^T}R_i^TR_j) \\ &= Log((f_{R_{ij}}(\bar{\delta}_{\omega,i}^b)Exp_R(\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ij}}}\Delta \delta_{\omega,i}^b))^TR_i^TR_j) \end{align} $$ where $f_{R_{ij}}(\delta_{\omega,i}^b) = \prod_{k=i}^{j-1}Exp_{R}(T(y_{\omega, k} - \delta_{\omega,i}^b))$ and $\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ij}}} = -\sum_{k=i}^{j-1}[f_{R_{k+1,j}}(\bar{\delta}_{\omega,i}^b)^T J_{r_k}T]$
where $f_{v_{ij}}(\delta_{a,i}^b, \delta_{\omega,i}^b) = \sum_{k=i}^{j-1}[T\Delta {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)]$, $\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ij}}} = -\sum_{k=i}^{j-1}[T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)]$ and $\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ij}}} = -\sum_{k=i}^{j-1}[T f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)[y_{a,k}-\bar{\delta}_{a,i}^b]_{\times}\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}]$
where $f_p(\delta_{a,i}^b, \delta_{\omega,i}^b) = \sum_{k=i}^{j-1}[T\Delta{\tilde{v_{ij}}}-\frac{1}{2}T^2 {\tilde{R}_{ik}}(y_{a,k}-\delta_{a,k}^b)]$, $\mathcal{J}_{\delta_{a,i}^b}^{f_{p_{ij}}}=\sum_{k=i}^{j-1}[T\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ik}}}\ - \frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)]$ and $\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{p_{ij}}}=\sum_{k=i}^{j-1}[T\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{v_{ik}}} - \frac{1}{2}T^2 f_{R_{ik}}(\bar{\delta}_{\omega,i}^b)[y_{a,k}-\bar{\delta}_{a,i}^b]_{\times}\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}]$
SO what values do we need to calculate between frame $i$ and $j$?
- $f_{R_{ik}}(\delta_{\omega,k}^b)$ and $\mathcal{J}_{\delta_{\omega,i}^b}^{f_{R_{ik}}}$
- $f_{v_{ik}}(\delta_{a,i}^b, \delta_{\omega,i}^b)$ and $\mathcal{J}_{\delta_{a,i}^b}^{f_{v_{ik}}}$ and $\mathcal{J}_{\delta_{\omega,i}^b}^{f_{v_{ik}}}$
- $f_{p}(\delta_{a,i}^b, \delta_{\omega,i}^b)$ and $\mathcal{J}_{\delta_{a,i}^b}^{f_{p_{ij}}}$ and $\mathcal{J}_{\delta_{\omega,,i}^b}^{f_{p_{ij}}}$
Another thing to pay attention is that we should use bias delta value as state.
One last thing need to mention is how sufficient small $\delta \phi$ need to be so that we can use? equations below? $$ \begin{align} Exp(\phi + \delta\phi) &\approx Exp(\phi)Exp(J_r(\phi)\delta\phi) \\ Exp(\phi + J_r^{-1}(\phi)\delta\phi) &\approx Exp(\phi)Exp(J_r(\phi)J_r^{-1}(\phi)\delta\phi) \\ Log(Exp(\phi)Exp(\delta\phi)) &\approx \phi + J_r^{-1}(\phi)\delta\phi \end{align} $$
# SO3 exponential approximation imports
from math import sin, cos
import numpy as np
from scipy.linalg import expm, logm
def skew(omega):
return np.array([[0, -omega[2].item(), omega[1].item()],
[omega[2].item(), 0, -omega[0].item()],
[-omega[1].item(), omega[0].item(), 0]])
def unskew(Omega):
return np.array([[Omega[2, 1].item()], [Omega[0, 2].item()], [Omega[1, 0].item()]])
def exp(omega):
return expm(skew(omega))
def log(R):
return unskew(logm(R))
def Adjoint_SO3(Omega):
return Omega
def LeftJacobian_SO3(w):
theta = np.linalg.norm(w)
A = skew(w)
if theta == 0:
return np.eye(3)
# Note: when theta is small, we should use taylor series
return np.eye(3) + ((1-cos(theta))/theta**2)*A + ((theta-sin(theta))/theta**3)*np.matmul(A, A)
#def RightJacobian_SO3(w):
# return Adjoint_SO3(expm(skew(-w))).dot(LeftJacobian_SO3(w))
def RightJacobian_SO3(w):
theta = np.linalg.norm(w)
A = skew(w)
if theta == 0:
return np.eye(3)
# Note: when theta is small, we should use taylor series
return np.eye(3) - ((1-cos(theta))/theta**2)*A + ((theta-sin(theta))/theta**3)*np.matmul(A, A)
def RightJacobian_SO3_inv(w):
theta = np.linalg.norm(w)
A = skew(w)
if theta == 0:
return np.eye(3)
# Note: when theta is small, we should use taylor series
return np.eye(3) + 0.5*A + (1/theta**2 + (1+cos(theta))/(2*theta*sin(theta)))*np.matmul(A, A)
def from_rpy(roll, pitch, yaw):
return rotz(yaw).dot(roty(pitch).dot(rotx(roll)))
def rotx(t):
c = np.cos(t)
s = np.sin(t)
return np.array([[1, 0, 0],
[0, c, -s],
[0, s, c]])
def roty(t):
c = np.cos(t)
s = np.sin(t)
return np.array([[c, 0, s],
[0, 1, 0],
[-s, 0, c]])
def rotz(t):
c = np.cos(t)
s = np.sin(t)
return np.array([[c, -s, 0],
[s, c, 0],
[0, 0, 1]])
#
## SO3 exponential approximation
import numpy as np
small_value_magnitude = 1E-2
def cal_exp_approximation_error(small_value_magnitude):
w = np.array([1, 2, 3])
small_w = np.array([1, 1, 1]) * small_value_magnitude
delta = log(exp(w + small_w).T.dot(exp(w).dot(exp(RightJacobian_SO3(w).dot(small_w)))))
return np.linalg.norm(delta)
print(f'error norm: {cal_exp_approximation_error(small_value_magnitude)}')
#