pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Files
89f25ad747f9a1fc0bb4149d32e34b38d5a3e209
openvins_linux/docs/propagation.dox
pi-dell 5ecdc6abcf initial commit
2022-08-05 08:23:25 +03:00

473 lines
20 KiB
Plaintext
Raw Blame History

/**
@page propagation Propagation
@tableofcontents
@section imu_measurements IMU Measurements
We use a 6-axis inertial measurement unit (IMU) to propagate the inertial navigation system (INS),
which provides measurements of the local rotational velocity (angular rate) \f$\boldsymbol{\omega}_m\f$ and
local translational acceleration \f$\mathbf{a}_m\f$:
\f{align*}{
\boldsymbol{\omega}_m(t) &= \boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\
\mathbf{a}_m(t) &= \mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t)
\f}
where \f$\boldsymbol{\omega}\f$ and \f$\mathbf{a}\f$ are the true rotational velocity and translational
acceleration in the IMU local frame \f$\{I\}\f$,
\f$\mathbf{b}_{g}\f$ and \f$\mathbf{b}_{a}\f$ are the gyroscope and accelerometer biases, and \f$\mathbf{n}_{{g}}\f$
\f$\mathbf{n}_{{a}}\f$ are white Gaussian noise,
\f${^G\mathbf{g}} = [ 0 ~~ 0 ~~ 9.81 ]^\top\f$ is the gravity expressed in the global frame \f$\{G\}\f$
(noting that the gravity is slightly different on different locations of the globe),
and \f$^I_G\mathbf{R}\f$ is the rotation matrix from global to IMU local frame.
@section ins_state State Vector
We define our INS state vector \f$\mathbf{x}_I\f$ at time \f$t\f$ as:
\f{align*}{
\mathbf{x}_I(t) =
\begin{bmatrix}
^I_G\bar{q}(t) \\
^G\mathbf{p}_I(t) \\
^G\mathbf{v}_I(t)\\
\mathbf{b}_{\mathbf{g}}(t) \\
\mathbf{b}_{\mathbf{a}}(t)
\end{bmatrix}
\f}
where \f$^I_G\bar{q}\f$ is the unit quaternion representing the rotation global to IMU frame,
\f$^G\mathbf{p}_I\f$ is the position of IMU in global frame,
and \f$^G\mathbf{v}_I\f$ is the velocity of IMU in global frame.
We will often write time as a subscript of \f$I\f$ describing the state of IMU at the time
for notation clarity (e.g., \f$^{I_t}_G\bar{q} = \text{}^I_G\bar{q}(t)\f$).
In order to define the IMU error state, the standard additive error definition is employed
for the position, velocity, and biases,
while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quaternion multiplicative error
\f$\otimes\f$:
\f{align*}{
^I_G\bar{q} &= \delta \bar{q} \otimes \text{}^I_G \hat{\bar{q}}\\
\delta \bar{q} &= \begin{bmatrix}
\hat{\mathbf{k}}\sin(\frac{1}{2}\tilde{\theta})\\
\cos(\frac{1}{2}\tilde{\theta}) \end{bmatrix}
\simeq
\begin{bmatrix}
\frac{1}{2}\tilde{\boldsymbol{\theta}}\\ 1
\end{bmatrix}
\f}
where \f$\hat{\mathbf{k}}\f$ is the rotation axis and \f$\tilde{\theta}\f$ is the rotation angle.
For small rotation, the error angle vector is approximated by \f$\tilde{\boldsymbol{\theta}} = \tilde{\theta}~\hat{\mathbf{k}}\f$
as the error vector about the three orientation axes.
The total IMU error state thus is defined as the following 15x1 (not 16x1) vector:
\f{align*}{
\tilde{\mathbf{x}}_I(t) =
\begin{bmatrix}
^I_G\tilde{\boldsymbol{\theta}}(t) \\
^G\tilde{\mathbf{p}}_I(t) \\
^G\tilde{\mathbf{v}}_I(t)\\
\tilde{\mathbf{b}}_{{g}}(t) \\
\tilde{\mathbf{b}}_{{a}}(t)
\end{bmatrix}
\f}
@section imu_kinematic IMU Kinematics
The IMU state evolves over time as follows
(see [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf) @cite Trawny2005TR).
\f{align*}{ \newcommand{\comm}[1]{}
^I_G\dot{\bar{q}}(t)
\comm{
&= \lim_{\delta t \to 0} \frac{1}{\delta t}
(^{I_{t + \delta t}}_G\bar{q} - \text{}^{I_{t}}_G\bar{q})\\
&= \lim_{\delta t \to 0} \frac{1}{\delta t}
(^{I_{t + \delta t}}_{L_{t}}\bar{q} \otimes ^{I_{t}}_{G}\bar{q}
- \bar{q}_0 \otimes \text{}^{I_{t}}_G\bar{q})\\
&= \lim_{\delta t \to 0} \frac{1}{\delta t}
(^{I_{t + \delta t}}_{L_{t}}\bar{q} - \bar{q}_0 ) \otimes \text{}^{I_{t}}_{G}\bar{q}\\
&\approx \lim_{\delta t \to 0} \frac{1}{\delta t}
\Bigg (\begin{bmatrix} \frac{1}{2}\delta \boldsymbol{\theta}\\ 1 \end{bmatrix}
-\begin{bmatrix} \boldsymbol{0}\\ 1 \end{bmatrix} \Bigg )
\otimes \text{}^{I_{t}}_{G}\bar{q} \\
&= \frac{1}{2} \begin{bmatrix} \boldsymbol{\omega}\\ 0 \end{bmatrix}
\otimes \text{}^{I_{t}}_{G}\bar{q}\\}
&= \frac{1}{2} \begin{bmatrix} -\lfloor \boldsymbol{\omega}(t) \times \rfloor
&& \boldsymbol{\omega}(t) \\
-\boldsymbol{\omega}^\top(t) && 0 \end{bmatrix} \text{}^{I_{t}}_{G}\bar{q}\\
&=: \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \text{}^{I_{t}}_{G}\bar{q}\\
^G\dot{\mathbf{p}}_I(t) &=\text{} ^G\mathbf{v}_I(t)\\
^G\dot{\mathbf{v}}_I(t) &=\text{} ^{I_{t}}_G\mathbf{R}^\top\mathbf{a}(t) \\
\dot{\mathbf{b}}_{\mathbf{g}}(t) &= \mathbf{n}_{wg}\\
\dot{\mathbf{b}}_{\mathbf{a}}(t) &= \mathbf{n}_{wa}
\f}
where we have modeled the gyroscope and accelerometer biases as random walk
and thus their time derivatives are white Gaussian.
Note that the above kinematics have been defined in terms of the *true* acceleration and angular velocities.
@section conti_prop Continuous-time IMU Propagation
Given the continuous-time measurements \f$\boldsymbol{\omega}_m(t)\f$
and \f$\mathbf{a}_m(t)\f$ in the time interval \f$t \in [t_k,t_{k+1}]\f$,
and their estimates, i.e. after taking the expectation,
\f$\hat{\boldsymbol{\omega}}(t) = \boldsymbol{\omega}_m(t) - \hat{\boldsymbol{b}}_g(t)\f$ and
\f$\hat{\boldsymbol{a}}(t) = \boldsymbol{a}_m(t) - \hat{\boldsymbol{b}}_a(t) - ^I_G\hat{\mathbf{R}}(t){}^G\mathbf{g}\f$,
we can define the solutions to the above IMU kinematics differential equation.
The solution to the quaternion evolution has the following general form:
\f{align*}{
^{I_{t}}_{G}\bar{q} = \boldsymbol{\Theta}(t,t_k) \text{}^{I_{k}}_{G}\bar{q}
\f}
Differentiating and reordering the terms yields the governing equation for
\f$\boldsymbol{\Theta}(t,t_k)\f$ as
\f{align*}{ \newcommand{\comm}[1]{}
\boldsymbol{\Theta}(t,t_k) &= \text{}^{I_t}_G\bar{q} \text{ }^{I_k}_{G}\bar{q}^{-1}\\
\Rightarrow \dot{\boldsymbol{\Theta}}(t,t_k) &=
\text{}^{I_t}_G\dot{\bar{q}} \text{ }^{I_k}_{G}\bar{q}^{-1}\\
&= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \text{ }^{I_t}_{G}\bar{q}
\text{ }^{I_k}_{G}\bar{q}^{-1}\\
&= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \boldsymbol{\Theta}(t,t_k)
\f}
with \f$\boldsymbol{\Theta}(t_k,t_k) = \mathbf{I}_{4}\f$.
If we take \f$\boldsymbol{\omega}(t) = \boldsymbol{\omega}\f$ to be constant over the the period
\f$\Delta t = t_{k+1} - t_k\f$,
then the above system is linear time-invarying (LTI), and
\f$\boldsymbol{\Theta}\f$ can be solved as (see [[Stochastic Models, Estimation, and Control]](https://books.google.com/books?id=L_YVMUJKNQUC) @cite Maybeck1982STOC):
\f{align*}{
\boldsymbol{\Theta}(t_{k+1},t_k)
&= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\Delta t\bigg)\\
&= \cos\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t \bigg) \cdot \mathbf{I}_4
+ \frac{1}{|\boldsymbol{\omega}|}\sin\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t \bigg)
\cdot \boldsymbol{\Omega}(\boldsymbol{\omega})\\
&\simeq
\mathbf{I}_4 + \frac{\Delta t}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})
\f}
where the approximation assumes small \f$|\boldsymbol{\omega}|\f$.
We can formulate the quaternion propagation from \f$t_k\f$ to \f$t_{k+1}\f$ using the estimated
rotational velocity \f$\hat{\boldsymbol{\omega}}(t) = \hat{\boldsymbol{\omega}}\f$ as:
\f{align*}{
\text{}^{I_{k+1}}_{G}\hat{\bar{q}}
= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})\Delta t\bigg)
\text{}^{I_{k}}_{G}\hat{\bar{q}}
\f}
Having defined the integration of the orientation, we can integrate the velocity and position over the measurement interval:
\f{align*}{
^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} + \int_{t_{k}}^{t_{k+1}}
{^G\hat{\mathbf{a}}(\tau)} d\tau \\
&= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t+ \int_{t_{k}}^{t_{k+1}}
{^G_{I_{\tau}}\hat{\mathbf{R}}(\mathbf{a}_m(\tau) - \hat{\mathbf{b}}_{\mathbf{a}}(\tau)) d\tau}\\[1em]
^G\hat{\mathbf{p}}_{I_{k+1}} &=
\text{}^G\hat{\mathbf{p}}_{I_k} + \int_{t_{k}}^{t_{k+1}}
{^G\hat{\mathbf{v}}_I(\tau)} d\tau \\
&= \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t
- \frac{1}{2}{}^G\mathbf{g}\Delta t^2 +
\int_{t_{k}}^{t_{k+1}} \int_{t_{k}}^{s}
{^G_{I_{\tau}}\hat{\mathbf{R}}(\mathbf{a}_m(\tau) - \hat{\mathbf{b}}_{\mathbf{a}}(\tau)) d\tau ds}
\f}
Propagation of each bias \f$\hat{\mathbf{b}}_{\mathbf{g}}\f$ and \f$\hat{\mathbf{b}}_{\mathbf{a}}\f$ is given by:
\f{align*}{
\hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k} + \int_{t_{k+1}}^{t_{k}}
{\hat{\mathbf{n}}_{wg}(\tau)} d\tau \\
&= \hat{\mathbf{b}}_{\mathbf{g},k} \\
\hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} + \int_{t_{k+1}}^{t_{k}}
{\hat{\mathbf{n}}_{wa}(\tau)} d\tau \\
&= \hat{\mathbf{b}}_{\mathbf{a},k}
\f}
The biases will not evolve since our random walk noises \f$\hat{\mathbf{n}}_{wg}\f$ and \f$\hat{\mathbf{n}}_{wa}\f$ are zero-mean white Gaussian.
All of the above integrals could be analytically or numerically solved if one wishes to use the continuous-time measurement evolution model.
@section disc_prop Discrete-time IMU Propagation
A simpler method is to model the measurements as discrete-time over the integration period.
To do this, the measurements can be assumed to be constant during the sampling period.
We employ this assumption and approximate that the measurement at time \f$t_k\f$ remains
the same until we get the next measurement at \f$t_{k+1}\f$.
For the quaternion propagation, it is the same as continuous-time propagation
with constant measurement assumption \f${\boldsymbol{\omega}}_{m}(t_k) = {\boldsymbol{\omega}}_{m,k}\f$.
We use subscript \f$k\f$ to denote it is the measurement we get at time \f$t_k\f$.
Therefore the propagation of quaternion can be written as:
\f{align*}{
\text{}^{I_{k+1}}_{G}\hat{\bar{q}}
= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg)
\text{}^{I_{k}}_{G}\hat{\bar{q}}
\f}
For the velocity and position propagation we have constant
\f$\mathbf{a}_{m}(t_k) = \mathbf{a}_{m,k}\f$ over \f$t \in [t_k, t_{k+1}]\f$.
We can therefore directly solve for the new states as:
\f{align*}{
^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t
+\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\
^G\hat{\mathbf{p}}_{I_{k+1}}
&= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t
- \frac{1}{2}{}^G\mathbf{g}\Delta t^2
+ \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2
\f}
The propagation of each bias is likewise the continuous system:
\f{align*}{
\hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k}\\
\hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k}
\f}
@section error_prop Discrete-time Error-state Propagation
In order to propagate the covariance matrix, we should derive the error-state propagation,
i.e., computing the system Jacobian \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and noise Jacobian \f$\mathbf{G}_{k}\f$.
In particular, when the covariance matrix of the continuous-time measurement noises is given by
\f$\mathbf{Q}_c\f$, then the discrete-time noise covariance \f$\mathbf{Q}_d\f$ can be computed as
(see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR Eq. (129) and (130)):
\f{align*}{
\sigma_{g} &= \frac{1}{\sqrt{\Delta t}}~ \sigma_{g_c} \\
\sigma_{bg} &= \sqrt{\Delta t}~ \sigma_{bg_c} \\[1em]
\mathbf{Q}_{meas} &=
\begin{bmatrix}
\frac{1}{\Delta t}~ \sigma_{g_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \frac{1}{\Delta t}~ \sigma_{a_c}^2~ \mathbf{I}_3
\end{bmatrix} \\
\mathbf{Q}_{bias} &=
\begin{bmatrix}
\Delta t~ \sigma_{bg_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \Delta t~ \sigma_{ba_c}^2~ \mathbf{I}_3
\end{bmatrix}
\f}
where \f$\mathbf{n} = [ \mathbf{n}_g ~ \mathbf{n}_a ~ \mathbf{n}_{bg} ~ \mathbf{n}_{ba} ]^\top\f$ are the discrete
IMU sensor noises which have been converted from their continuous representations.
We define the stacked discrete measurement noise as follows:
\f{align*}{
\mathbf{Q}_{d} &=
\begin{bmatrix}
\mathbf{Q}_{meas} & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{Q}_{bias}
\end{bmatrix}
\f}
The method of computing Jacobians is to "perturb" each variable in the system and see how the old error "perturbation" relates to the new error state.
That is,
\f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ can be found by perturbing each variable as:
\f{align*}{
\tilde{\mathbf{x}}_I(t_{k+1}) = \boldsymbol{\Phi}(t_{k+1},t_k) \tilde{\mathbf{x}}_I(t_{k}) + \mathbf{G}_{k} \mathbf{n}
\f}
For the orientation error propagation, we start with the \f$\mathbf{SO}(3)\f$ perturbation using
\f${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}\f$:
\f{align*}{
{}^{I_{k+1}}_G \mathbf{R} &= \text{}^{I_{k+1}}_{I_{k}} \mathbf{R} \text{}^{I_{k}}_G \mathbf{R} \\
(\mathbf{I}_3 - \lfloor ^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k+1}}_{G}
\hat{\mathbf{R}}
&\approx \textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t)
(\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G}
\hat{\mathbf{R}}\\
&=\textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\textrm{exp}(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t)
(\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G}
\hat{\mathbf{R}}\\
&=\text{}^{I_{k+1}}_{I_{k}} \hat{\mathbf{R}}
(\mathbf{I}_3 - \lfloor \mathbf J_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)
\tilde{\boldsymbol{\omega}}_k\Delta t \times\rfloor)
(\mathbf{I}_3 - \lfloor ^{I_k}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)
\text{}^{I_{k}}_G \hat{\mathbf{R}}
\f}
where \f$\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} - \hat{\boldsymbol{\omega}}
= -(\tilde{\mathbf{b}}_{\mathbf{g}} + \mathbf{n}_g)\f$ handles both the perturbation to the bias and measurement noise.
\f$\mathbf {J}_r(\boldsymbol{\theta})\f$ is the right Jacobian of \f$\mathbf{SO}(3)\f$
that maps the variation of rotation angle in the parameter vector space into the variation in the tangent vector space to the manifold
[see @ref ov_core::Jr_so3()].
By neglecting the second order terms from above, we obtain the following orientation error propagation:
\f{align*}
\text{}^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}} \approx
\text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} \text{}^{I_k}_{G}\tilde{\boldsymbol{\theta}}
- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}})
\Delta t (\tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{\mathbf{g},k})
\f}
Now we can do error propagation of position and velocity using the same scheme:
\f{align*}{
^G\mathbf{p}_{I_{k+1}}
&= \text{}^G\mathbf{p}_{I_k} + \text{}^G\mathbf{v}_{I_k} \Delta t
- \frac{1}{2}{}^G\mathbf{g}\Delta t^2
+ \frac{1}{2}\text{}^{I_k}_G\mathbf{R}^\top \mathbf{a}_{k}\Delta t^2\\
^G\hat{\mathbf{p}}_{I_{k+1}} + \text{}^G\tilde{\mathbf{p}}_{I_{k+1}}
&\approx \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\tilde{\mathbf{p}}_{I_k}
+ \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t
+ \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t
- \frac{1}{2}{}^G\mathbf{g}\Delta t^2\\
&\hspace{4cm} + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top
(\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)
(\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t^2\\
\\
^G\mathbf{v}_{k+1} &= \text{}^G\mathbf{v}_{I_k} - {}^G\mathbf{g}\Delta t
+\text{}^{I_k}_G\mathbf{R}^\top\mathbf{a}_{k}\Delta t\\
^G\hat{\mathbf{v}}_{k+1} + ^G\tilde{\mathbf{v}}_{k+1} &\approx
{}^G\hat{\mathbf{v}}_{I_k} + {}^G\tilde{\mathbf{v}}_{I_k}
- {}^G\mathbf{g}\Delta t
+ \text{}^{I_k}_G\hat{\mathbf{R}}^\top
(\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)
(\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t
\f}
where \f$\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}}
= - (\tilde{\mathbf{b}}_{\mathbf{a}} + \mathbf{n}_{\mathbf{a}})\f$.
By neglecting the second order error terms, we obtain the following position and velocity error propagation:
\f{align*}{
\text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &=
\text{}^G\tilde{\mathbf{p}}_{I_k}
+ \Delta t \text{}^G\tilde{\mathbf{v}}_{I_k}
- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top
\lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor
^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}
- \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2
(\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k})\\
^G\tilde{\mathbf{v}}_{k+1} &=
\text{}^G\tilde{\mathbf{v}}_{I_k}
- \text{}^{I_k}_G\hat{\mathbf{R}}^\top
\lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor
^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}
- \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t
(\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k})
\f}
The propagation of the two random-walk biases are as follows:
\f{align*}{
\mathbf{b}_{\mathbf{g},k+1} &= \mathbf{b}_{\mathbf{g},k} + \mathbf{n}_{wg} \\
\hat{\mathbf{b}}_{\mathbf{g},k+1} + \tilde{\mathbf{b}}_{\mathbf{g},k+1} &=
\hat{\mathbf{b}}_{\mathbf{g},k} + \tilde{\mathbf{b}}_{\mathbf{g},k}
+ \mathbf{n}_{wg} \\
\tilde{\mathbf{b}}_{\mathbf{g},k+1} &=
\tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{wg} \\[1em]
\mathbf{b}_{\mathbf{a},k+1} &= \mathbf{b}_{\mathbf{a},k} + \mathbf{n}_{wa} \\
\hat{\mathbf{b}}_{\mathbf{a},k+1} + \tilde{\mathbf{b}}_{\mathbf{a},k+1} &=
\hat{\mathbf{b}}_{\mathbf{a},k} + \tilde{\mathbf{b}}_{\mathbf{a},k}
+ \mathbf{n}_{wa} \\
\tilde{\mathbf{b}}_{\mathbf{a},k+1} &=
\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{wa}
\f}
By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices as:
\f{align*}{
\boldsymbol{\Phi}(t_{k+1},t_k) &=
\begin{bmatrix}
\text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} & \mathbf{0}_3 & \mathbf{0}_3 &
- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}})
\Delta t & \mathbf{0}_3 \\
- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor
& \mathbf{I}_3 & \Delta t \mathbf{I}_3 & \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 \\
- \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor
& \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3
\end{bmatrix}
\f}
\f{align*}{
\mathbf{G}_{k} &=
\begin{bmatrix}
- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}})
\Delta t & \mathbf{0}_3
& \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2
& \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t
& \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3
& \mathbf{I}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3
& \mathbf{0}_3 & \mathbf{I}_3
\end{bmatrix}
\f}
Now, with the computed \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices,
we can propagate the covariance from \f$t_k\f$ to \f$t_{k+1}\f$:
\f{align*}{
\mathbf{P}_{k+1|k} = \boldsymbol{\Phi}(t_{k+1},t_k)\mathbf{P}_{k|k}\boldsymbol{\Phi}(t_{k+1},t_k)^\top + \mathbf{G}_k\mathbf{Q}_d\mathbf{G}_k^\top
\f}
*/
Reference in New Issue View Git Blame Copy Permalink