Robot State Estimation

Abstract

Micro aerial vehicles have gained significant attention in the last decade both in academia and industry, mostly due to their potential use in a wide range of applications such as exploration [46], inspection [52], mapping, interaction with the environment [7, 44], search and rescue [27], and to their significant mechanical simplicity and ease of control. Moreover, their ability to operate in confined spaces, hover in space and even perch, together with a decrease in cost make them very attractive with tremendous potential as UAM flying platforms.

Appendices

Appendix 2.A Error State Kinematics (Continous Time)

To obtain the error-state kinematic equations we have to write each state equation in (2.28) as its composition of nominal- and error-state, solve for the error-state and simplify all second-order infinitesimals. For the terms $\delta {\dot{\varvec{p}}}$, $\delta {\dot{\varvec{a}}}_{b}$, $\delta {\dot{\varvec{\omega }}}_{b}$ this operation is trivial. In contrast, for the terms involving rotation elements ($\delta {\dot{\varvec{v}}}$ and $\delta {\dot{\varvec{q}}}$) require some non-trivial manipulations which are detailed in the following depending the orientation error definition (GE or LE).

2.1.1 2.A.1 Globally-Defined Orientation Error (GE)

2.1.1.1 2.A.1.1 GE: Linear Velocity Error

Considering the true acceleration as large- and small-signal accelerations in body frame ${\varvec{a}}_{Bt} = {\varvec{a}}_{B} + \delta {\varvec{a}}_{B}$ with

$$\begin{aligned}&{\varvec{a}}_{B} \triangleq {\varvec{a}}_s-{\varvec{a}}_{b} \,, \end{aligned}$$

(2.93a)

$$\begin{aligned}&\delta {\varvec{a}}_{B} \triangleq -\delta {\varvec{a}}_{b}-{\varvec{a}}_n \,, \end{aligned}$$

(2.93b)

and substituting in (2.28b) we have

$$\begin{aligned} \dot{\varvec{v}}_t = {\varvec{R}}_t\,({\varvec{a}}_{B} + \delta {\varvec{a}}_{B}) + {\varvec{g}}\,. \end{aligned}$$

(2.94)

Defining ${\varvec{v}}_{t}$ as the composition of the nominal- plus the error-state, and ${\varvec{R}}_t$ with its small signal approximation (i.e., ${\varvec{R}}_t\,=\,({\varvec{I}}+ [\delta {\varvec{\theta }}]_\times ){\varvec{R}}+ {\varvec{O}}(||\delta {\varvec{\theta }}||^2)$) ignoring the infinitesimal term ${\varvec{O}}(||\delta {\varvec{\theta }}||^2)$, we end up with

$$\begin{aligned} \dot{\varvec{v}}+ \delta {\dot{\varvec{v}}} = ({\varvec{I}}+ [\delta {\varvec{\theta }}]_\times ){\varvec{R}}\,({\varvec{a}}_B + \delta {\varvec{a}}_B) + {\varvec{g}}\,. \end{aligned}$$

(2.95)

Substituting the nominal velocity $\dot{\varvec{v}}$ by (2.30b), then using (2.93a) and finally rearranging terms, we have

$$\begin{aligned} \delta {\dot{\varvec{v}}} = {\varvec{R}}\delta {\varvec{a}}_B + [\delta {\varvec{\theta }}]_\times {\varvec{R}}({\varvec{a}}_B + \delta {\varvec{a}}_B)\,. \end{aligned}$$

(2.96)

Reorganizing some cross-products (with $[{\varvec{a}}]_\times {\varvec{b}}= - [{\varvec{b}}]_\times {\varvec{a}}$), we get

$$\begin{aligned} \delta {\dot{\varvec{v}}} = {\varvec{R}}\delta {\varvec{a}}_B - [{\varvec{R}}{\varvec{a}}_B]_\times \delta {\varvec{\theta }}\,, \end{aligned}$$

(2.97)

which recalling (2.93a) and (2.93b), and rearranging leads to

$$\begin{aligned} \delta {\dot{\varvec{v}}} = -[{\varvec{R}}({\varvec{a}}_s - {\varvec{a}}_b)]_\times \delta {\varvec{\theta }}- {\varvec{R}}\delta {\varvec{a}}_b - {\varvec{R}}{\varvec{a}}_n\,. \end{aligned}$$

(2.98)

Assuming accelerometers noise as white, uncorrelated and isotropic, we obtain the dynamics of the linear velocity error

$$\begin{aligned} \delta {\dot{\varvec{v}}} = -[{\varvec{R}}({\varvec{a}}_s - {\varvec{a}}_b)]_\times \delta {\varvec{\theta }}- {\varvec{R}}\delta {\varvec{a}}_b - {\varvec{a}}_n\,. \end{aligned}$$

(2.99)

2.1.1.2 2.A.1.2 GE: Orientation Error

With the true quaternion ${\varvec{q}}_t$ as a composition of the nominal- and error-state rotations, and its derivation from (2.10), we have

$$\begin{aligned} \dot{\varvec{q}}_t&= \dot{(\delta {\varvec{q}}\otimes {\varvec{q}})} \end{aligned}$$

(2.100a)

$$\begin{aligned}&= \delta \dot{{\varvec{q}}}\otimes {\varvec{q}}+ {\delta {\varvec{q}}}\otimes \dot{\varvec{q}}\end{aligned}$$

(2.100b)

$$\begin{aligned}&= \delta \dot{{\varvec{q}}}\otimes {\varvec{q}}+ \frac{1}{2}\delta {\varvec{q}}\otimes {\varvec{q}}\otimes {\varvec{\omega }}\,. \end{aligned}$$

(2.100c)

Similarly, we can define

$$\begin{aligned} \dot{\varvec{q}}_t&= \frac{1}{2}{\varvec{q}}_t\otimes {\varvec{\omega }}_t \end{aligned}$$

(2.101a)

$$\begin{aligned}&= \frac{1}{2}{\delta {\varvec{q}}}\otimes {\varvec{q}}\otimes {\varvec{\omega }}_t \,. \end{aligned}$$

(2.101b)

Matching (2.100c) with (2.101b), and having ${\varvec{\omega }}_t = {\varvec{\omega }}+ \delta {\varvec{\omega }}$, this reduces to

$$\begin{aligned} \delta \dot{{\varvec{q}}}\otimes {\varvec{q}}= \frac{1}{2}{\delta {\varvec{q}}}\otimes {\varvec{q}}\otimes \delta {\varvec{\omega }}\,. \end{aligned}$$

(2.102)

Right-multiplying left and right terms by ${\varvec{q}}^*$, and recalling that ${\varvec{q}}\otimes \delta {\varvec{\omega }}\otimes {\varvec{q}}^* \equiv {\varvec{R}}\delta {\varvec{\omega }}$, we can further develop as follows

$$\begin{aligned} \delta \dot{{\varvec{q}}}&= \frac{1}{2}{\delta {\varvec{q}}}\otimes {\varvec{q}}\otimes \delta {\varvec{\omega }}\otimes {\varvec{q}}^* \end{aligned}$$

(2.103a)

$$\begin{aligned}&= \frac{1}{2}{\delta {\varvec{q}}}\otimes ({\varvec{R}}\delta {\varvec{\omega }}) \end{aligned}$$

(2.103b)

$$\begin{aligned}&= \frac{1}{2}{\delta {\varvec{q}}}\otimes \delta {\varvec{\omega }}_G\,, \end{aligned}$$

(2.103c)

with $\delta {\varvec{\omega }}_G \equiv {\varvec{R}}\delta {\varvec{\omega }}$ the small-signal angular rate expressed in the global frame. Then,

$$\begin{aligned} 2\delta \dot{\varvec{q}}&= \delta {\varvec{q}}\otimes \delta {\varvec{\omega }}_G\end{aligned}$$

(2.104a)

$$\begin{aligned}&= {\varvec{\varOmega }}(\delta {\varvec{\omega }}_G)\delta {\varvec{q}}\end{aligned}$$

(2.104b)

$$\begin{aligned}&= \begin{bmatrix} 0&-\delta {\varvec{\omega }}_G^\top \\ \delta {\varvec{\omega }}_G&-[\delta {\varvec{\omega }}_G]_\times \end{bmatrix} \begin{bmatrix} 1 \\ \delta {\varvec{\theta }}/2 \end{bmatrix}\,. \end{aligned}$$

(2.104c)

Discarding the first row, which is not very useful, we can extract

$$\begin{aligned} \delta \dot{{\varvec{\theta }}} = \delta {\varvec{\omega }}_G - \frac{1}{2}[\delta {\varvec{\omega }}_G]_\times \delta {\varvec{\theta }}\,, \end{aligned}$$

(2.105)

where also removing the second order infinitesimal terms

$$\begin{aligned} \delta \dot{{\varvec{\theta }}} = \delta {\varvec{\omega }}= {\varvec{R}}\delta {\varvec{\omega }}\,, \end{aligned}$$

(2.106)

where recalling (2.119b) we obtain the linearized dynamics of the global angular error,

$$\begin{aligned} \delta \dot{{\varvec{\theta }}} = -{\varvec{R}}\delta {\varvec{\omega }}_b -{\varvec{R}}\delta {\varvec{\omega }}_n\,. \end{aligned}$$

(2.107)

2.1.2 2.A.2 Locally-Defined Orientation Error (LE)

2.1.2.1 2.A.2.1 LE: Linear Velocity Error

Starting from (2.94) and defining ${\varvec{v}}_{t}$ as the composition of the nominal- plus the error-state, and ${\varvec{R}}_t$ with its small signal approximation (i.e., ${\varvec{R}}_t\,=\,{\varvec{R}}\,({\varvec{I}}+ [\delta {\varvec{\theta }}]_\times ) + {\varvec{O}}(||\delta {\varvec{\theta }}||^2)$) ignoring the infinitesimal term ${\varvec{O}}(||\delta {\varvec{\theta }}||^2)$, we end up with

$$\begin{aligned} \dot{\varvec{v}}+ \delta {\dot{\varvec{v}}} = {\varvec{R}}\,({\varvec{I}}+ [\delta {\varvec{\theta }}]_\times )\,({\varvec{a}}_B + \delta {\varvec{a}}_B) + {\varvec{g}}\,. \end{aligned}$$

(2.108)

Substituting the nominal velocity $\dot{\varvec{v}}$ by (2.30b), then using (2.93a) and finally rearranging terms, we have

$$\begin{aligned} \delta {\dot{\varvec{v}}} = {\varvec{R}}[\delta {\varvec{\theta }}]_\times {\varvec{a}}_B + {\varvec{R}}\delta {\varvec{a}}_B + {\varvec{R}}[\delta {\varvec{\theta }}]_\times \,\delta {\varvec{a}}_B\,. \end{aligned}$$

(2.109)

Reorganizing some cross-products (with $[{\varvec{a}}]_\times {\varvec{b}}= - [{\varvec{b}}]_\times {\varvec{a}}$), we get

$$\begin{aligned} \delta {\dot{\varvec{v}}} = {\varvec{R}}(\delta {\varvec{a}}_B - [{\varvec{a}}_B]_\times \delta {\varvec{\theta }})\,, \end{aligned}$$

(2.110)

which recalling (2.93a) and (2.93b), and rearranging leads to

$$\begin{aligned} \delta {\dot{\varvec{v}}} = -{\varvec{R}}[{\varvec{a}}_s - {\varvec{a}}_b]_\times \delta {\varvec{\theta }}- {\varvec{R}}\delta {\varvec{a}}_b - {\varvec{R}}{\varvec{a}}_n\,. \end{aligned}$$

(2.111)

Assuming accelerometers noise as white, uncorrelated and isotropic, we obtain the dynamics of the linear velocity error

$$\begin{aligned} \delta {\dot{\varvec{v}}} = -{\varvec{R}}[{\varvec{a}}_s - {\varvec{a}}_b]_\times \delta {\varvec{\theta }}- {\varvec{R}}\delta {\varvec{a}}_b - {\varvec{a}}_n\,. \end{aligned}$$

(2.112)

2.1.2.2 2.A.2.2 LE: Orientation Error

With the true quaternion ${\varvec{q}}_t$ as a composition of the nominal- and error-state rotations, and the quaternion derivative from (2.10), we have

$$\begin{aligned} \dot{\varvec{q}}_t&= \dot{({\varvec{q}}\otimes \delta {\varvec{q}})} \end{aligned}$$

(2.113a)

$$\begin{aligned}&= \dot{\varvec{q}}\otimes \delta {\varvec{q}}+ {\varvec{q}}\otimes \delta \dot{{\varvec{q}}} \end{aligned}$$

(2.113b)

$$\begin{aligned}&= \frac{1}{2}{\varvec{q}}\otimes {\varvec{\omega }}\otimes \delta {\varvec{q}}+ {\varvec{q}}\otimes \delta \dot{{\varvec{q}}} \,. \end{aligned}$$

(2.113c)

Similarly, we can define

$$\begin{aligned} \dot{\varvec{q}}_t&= \frac{1}{2}{\varvec{q}}_t\otimes {\varvec{\omega }}_t \end{aligned}$$

(2.114a)

$$\begin{aligned}&= \frac{1}{2}{\varvec{q}}\otimes {\delta {\varvec{q}}}\otimes {\varvec{\omega }}_t \,. \end{aligned}$$

(2.114b)

Matching (2.113c) with (2.114b), simplifying the leading ${\varvec{q}}$ and isolating $\delta \dot{\varvec{q}}$, we get

$$\begin{aligned} 2\delta \dot{\varvec{q}}= \delta {\varvec{q}}\otimes {\varvec{\omega }}_t - {\varvec{\omega }}\otimes \delta {\varvec{q}}\,, \end{aligned}$$

(2.115)

where substituting (2.4) and (2.7c), we end up with

$$\begin{aligned} 2\delta \dot{\varvec{q}}&= ^-{\varvec{Q}}({\varvec{\omega }}_t)\delta {\varvec{q}}- ^+{\varvec{Q}}({\varvec{\omega }})\delta {\varvec{q}}\end{aligned}$$

(2.116a)

$$\begin{aligned}&= \left( ^-{\varvec{Q}}({\varvec{\omega }}_t) - ^+{\varvec{Q}}({\varvec{\omega }})\right) \begin{bmatrix} 1 \\ \delta {\varvec{\theta }}/2 \end{bmatrix} \end{aligned}$$

(2.116b)

$$\begin{aligned}&= \begin{bmatrix} 0&-({\varvec{\omega }}_t-{\varvec{\omega }})^\top \\ ({\varvec{\omega }}_t-{\varvec{\omega }})&-[{\varvec{\omega }}_t+{\varvec{\omega }}]_\times \end{bmatrix} \begin{bmatrix} 1 \\ \delta {\varvec{\theta }}/2 \end{bmatrix} \end{aligned}$$

(2.116c)

$$\begin{aligned}&= \begin{bmatrix} 0&-\delta {\varvec{\omega }}^\top \\ \delta {\varvec{\omega }}&-[2{\varvec{\omega }}+\delta {\varvec{\omega }}]_\times \end{bmatrix} \begin{bmatrix} 1 \\ \delta {\varvec{\theta }}/2 \end{bmatrix}\,. \end{aligned}$$

(2.116d)

Simplifying again with (2.7c) the derivative term, we obtain

$$\begin{aligned} \begin{bmatrix} 0 \\ \delta \dot{{\varvec{\theta }}} \end{bmatrix} = \begin{bmatrix} 0&-\delta {\varvec{\omega }}^\top \\ \delta {\varvec{\omega }}&-[2{\varvec{\omega }}+\delta {\varvec{\omega }}]_\times \end{bmatrix} \begin{bmatrix} 1 \\ \delta {\varvec{\theta }}/2 \end{bmatrix}\,. \end{aligned}$$

(2.117)

Discarding the first row, which is not very useful, we can extract

$$\begin{aligned} \delta \dot{{\varvec{\theta }}} = \delta {\varvec{\omega }}- [{\varvec{\omega }}]_\times \delta {\varvec{\theta }}- \frac{1}{2}[\delta {\varvec{\omega }}]_\times \delta {\varvec{\theta }}\,, \end{aligned}$$

(2.118)

where also removing the second order infinitesimal terms and considering the true angular velocities as a composition of its large- and small-signal elements ${\varvec{\omega }}_t = {\varvec{\omega }}+ \delta {\varvec{\omega }}$ with

$$\begin{aligned}&{\varvec{\omega }}\triangleq {\varvec{\omega }}_s-{\varvec{\omega }}_{b} \,, \end{aligned}$$

(2.119a)

$$\begin{aligned}&\delta {\varvec{\omega }}\triangleq -\delta {\varvec{\omega }}_{b}-{\varvec{\omega }}_n \,, \end{aligned}$$

(2.119b)

we obtain the dynamics of the orientation error

$$\begin{aligned} \delta \dot{{\varvec{\theta }}} = -[{\varvec{\omega }}_s - {\varvec{\omega }}_b]_\times \delta {\varvec{\theta }}- \delta {\varvec{\omega }}_b - \delta {\varvec{\omega }}_n\,. \end{aligned}$$

(2.120)

Appendix 2.B Rotation Matrix Partial Derivatives

In order to differentiate the rotation of a vector, , it is convenient to use the matrix form ${\varvec{R}}({\varvec{q}})$. Herein, we compute an example of the derivatives of the function

(2.121)

with ${\varvec{n}}$ the measurement noise assumed to be Gaussian with zero mean and covariance matrix ${\varvec{n}}$. The corresponding derivatives are detailed in the following sections depending on whether they are computed with respect to the quaternion value (used in EKF) or with the orientation error with minimal representation (used in ESKF).

2.1.1 2.B.1 Partial Derivative w.r.t. Quaternion

To compute $\partial h({\varvec{x}})/\partial {\varvec{q}}$ (the subscripts t is avoided here for clarity) is more convenient to express the derivatives w.r.t. each component i of the quaternion ${\varvec{q}}$ such as

(2.122)

With the definition of (2.9) we get

(2.123a)

(2.123b)

(2.123c)

(2.123d)

2.1.2 2.B.2 Partial Derivative w.r.t. Rotation Error Using Minimal Representation

We now want to compute $\partial h({\varvec{x}}_t)/\partial \delta {\varvec{\theta }}$, which will depend on the orientation error representation (GE or LE) as explained in the following.

2.1.2.1 2.B.2.1 Globally-Defined Orientation Error (GE)

If we define the orientation error globally (i.e.  ${\varvec{q}}_t = \delta {\varvec{q}}\otimes {\varvec{q}}$) and with minimal representation $\delta {\varvec{\theta }}$ as shown in (2.7c), we have

(2.124)

Taking advantage of the rotation matrix approximation ${\varvec{R}}({\delta {\varvec{\theta }}})^\top \approx ({\varvec{I}}-[\delta {\varvec{\theta }}]_\times )$, we end up with

(2.125)

Thus, the derivative with respect to the orientation error is

(2.126)

2.1.2.2 2.B.2.2 Locally-Defined Orientation Error (LE)

Similarly to the previous derivation, if the orientation error is defined locally (i.e. ${\varvec{q}}_t = {\varvec{q}}\otimes \delta {\varvec{q}}$) and with minimal representation, we have

(2.127)

Using the rotation matrix approximation ${\varvec{R}}({\delta {\varvec{\theta }}})^\top \approx ({\varvec{I}}-[\delta {\varvec{\theta }}]_\times )$, we end up with

(2.128)

So, the derivative with respect to the orientation error is

(2.129)