Observability Analysis of a Matrix Kalman Filter-Based Navigation System Using Visual/Inertial/Magnetic Sensors

Abstract

A matrix Kalman filter (MKF) has been implemented for an integrated navigation system using visual/inertial/magnetic sensors. The MKF rearranges the original nonlinear process model in a pseudo-linear process model. We employ the observability rank criterion based on Lie derivatives to verify the conditions under which the nonlinear system is observable. It has been proved that such observability conditions are: (a) at least one degree of rotational freedom is excited, and (b) at least two linearly independent horizontal lines and one vertical line are observed. Experimental results have validated the correctness of these observability conditions.

1. Introduction

Inertial navigation systems (INS) have been widely used in many systems, such as ground vehicles, airplanes, helicopters, robotic systems, etc. However, INS drift will lead to exponential growth of errors in the navigation solutions due to the double integration of acceleration signals within inertial navigation computations. Therefore, to overcome such a problem, it is a very common practice to integrate INS with other sensors, which can calibrate the inertial sensor errors. In most outdoor applications, a Kalman filter estimator can be used for optimally combining both IMU and GPS measurements [1]. However, an indoor navigation system cannot use GPS since its signals are not available.

An alternative approach to calibrate INS errors is via the use of other sensors, such as cameras and magnetic sensors. Combining these two sensors to form a vision-aided inertial navigation system (V-INS) has recently become a popular topic of research [2]. By sensing the Earth's magnetic field a magnetic sensor can provide a drift-free heading estimate. Accurate 3-D orientation estimates of a rigid body by inertial/magnetic sensing were exploited in [3], where the aiding sensors (accelerometer and magnetic sensor) helped mitigate low-frequency gyro drift errors, while, in turn, the signals from the aiding sensors, which are prone to relatively high-frequency errors, are smoothed using gyro data. They are all based on the concept of vector matching, which requires, in principle, the measurements of constant reference vectors (e.g., gravity and the Earth's magnetic field) [4].

In this paper, we present a matrix Kalman filter (MKF) in which the estimate of the state matrix is expressed in terms of the matrix parameters of the original plant. The MKF has the statistical properties of the ordinary EKF, while retaining the advantages of a compact matrix notation by expressing the estimated matrix in terms of the original plant parameters [5].

The major contribution of this paper is to elucidate under which conditions a MKF-based nonlinear system for indoor navigation using visual/inertial/magnetic sensors is observable; in other words, the conditions when sufficient information is available for estimating a state matrix that contains, in the present case, the body attitude matrix, the gyro bias vector, relative velocity vector, the dual part of landmark and the magnetic variation superimposed to the magnetic reference vector. For the purpose of orientation determination, an accurately known homogeneous magnetic field in the environment is needed. Magnetic homogeneity is difficult to achieve, especially indoors, due to the presence of iron construction materials in floors, walls and ceilings, or to interferences from various types of equipment. In order to compensate for magnetic variations, a first-order Gauss-Markov vector random process is chosen to model the magnetic variation. To the best of our knowledge, there has been no such observability analysis so far for the integrated navigation systems in 3-D. We have extended the current work for the observability analysis for an orientation system described in [6], to the 3-D navigation systems based on inertial/visual/magnetic sensors.

2. Sensor Modeling

2.1. Inertial Sensor

Without loss of generality, the navigation reference frame is selected as the local-level frame (North-East-Down). Denote the navigation frame by n, the INS body frame by b, the camera frame by c and the inertial frame by i. Using gyros/accelerometers outputs, the relative velocity vⁿ and the body attitude matrix ${\mathbf{\text{C}}}_n^b$ satisfy the kinematic equations as [1,7]:

$${\dot{\mathbf{\xi }}}^n=-{\mathbf{\text{v}}}^n$$

(1)

$${\dot{\mathbf{v}}}^n={\mathbf{\text{C}}}_{\text{b}}^n\left({\mathbf{\text{f}}}^b-{\mathbf{b}}_a\right)+{\mathbf{\text{g}}}^n$$

(2)

$${\dot{\mathbf{\text{C}}}}_n^{\text{b}}=-\left[{\mathbf{\omega }}_{nb}^b\times \right]{\mathbf{C}}_n^b,{\mathbf{\omega }}_{nb}^b={\mathbf{\omega }}_{ib}^b-{\mathbf{b}}_g$$

(3)

$${\dot{\mathbf{b}}}_g={\mathbf{\text{n}}}_{\omega g}$$

(4)

where ξⁿ is an arbitrary point on the observed line, r^b is the lever arm from the IMU to the camera, as shown in Figure 1. b_a and b_g are 3 × 1 vectors that describe the biases affecting the accelerometer and gyro measurements, respectively. b_a can be compensated on time scales up to few hours, using the procedure described in [8]. b_g are modeled as random walk processes, driven by the white Gaussian noise vectors n_ωg. $\left[{\mathbf{\omega }}_{nb}^b\times \right]$ is the skew symmetric matrix of ${\mathbf{\omega }}_{nb}^b$. In the context of MEMS sensors, the component in the gyro output due to the Earth's rotation can be neglected as compared to the sensor errors.

2.2. Visual Sensor

The line point is taken as line representation, which is defined as the intersection of a line feature with a line passing through the image origin that is perpendicular to the line feature. The line point is unique for all lines except the lines passing through the origin. The line point is calculated by Goddard as [9]:

$$x_{lp}=\frac{f{m}_x^c{m}_z^c}{m_x^{c2}+m_y^{c2}},y_{lp}=\frac{f{m}_y^c{m}_z^c}{m_x^{c2}+m_y^{c2}}$$

(5)

Line features are represented by quaternion. Î = l + εm, where l is the unit direction vector of the observed line and m is related to the position by m = p × 1. In the n-frame:

$${\mathbf{\text{m}}}^n={\mathbf{\xi }}^n\times {\mathbf{\text{l}}}^n$$

(6)

while in the c-frame:

$$\begin{array}{cc}\hfill {\mathbf{\text{m}}}^c& =\left({\mathbf{\text{C}}}_n^c\left({\mathbf{\xi }}^n-{\mathbf{\text{C}}}_n^c{\mathbf{\text{r}}}^b\right)\right)\times \left({\mathbf{\text{C}}}_n^c{\mathbf{\text{l}}}^n\right)={\mathbf{\text{C}}}_n^c\left(\left({\mathbf{\xi }}^n-{\mathbf{\text{C}}}_n^c{\mathbf{\text{r}}}^b\right)\times {\mathbf{\text{l}}}^n\right)\hfill \\ & ={\mathbf{\text{C}}}_n^c{\mathbf{\text{m}}}^n-{\mathbf{\text{C}}}_n^c{\mathbf{\text{r}}}^b\times {\mathbf{\text{l}}}^n\approx {\mathbf{\text{C}}}_n^c{\mathbf{\text{m}}}^n\hfill \end{array}$$

(7)

where r^b can be ignored when the IMU and the camera are mounted closely together.

If relative position ξⁿ is orthogonal to lⁿ, that is, ξⁿ·lⁿ = 0, then we can obtain:

$$\Vert {\mathbf{\text{m}}}^n\Vert =\Vert {\mathbf{\xi }}^n\Vert \Vert {\mathbf{\text{l}}}^n\Vert =\Vert {\mathbf{\xi }}^n\Vert$$

(8)

That is to say, the norm of mⁿ is the minimum distance from the vehicle to the observed line. Using Equation (1), we obtain:

$${\dot{\mathbf{m}}}^n={\mathbf{\xi }}^n\times {\mathbf{\text{l}}}^n=-{\mathbf{\text{v}}}^n\times {\mathbf{\text{l}}}^n=[{\mathbf{\text{l}}}^n\times ]{\mathbf{\text{v}}}^n$$

(9)

For simplification, only vertical lines ${\mathbf{\text{l}}}_v^n$ and horizontal lines ${\mathbf{\text{l}}}_h^n$ are chosen as landmarks. According to Equation (5), we obtain:

$${\mathbf{\text{m}}}^c={\left[\begin{array}{ccc}{m}_x& m_y& m_z\end{array}\right]}^T={\left[\begin{array}{ccc}\frac{f{x}_{lp}{m}_z}{{x_{lp}}^2+{y_{lp}}^2}& \frac{f{y}_{lp}{m}_z}{{x_{lp}}^2+{y_{lp}}^2}& m_2\end{array}\right]}^T$$

(10)

A monocular camera is not enough to calculate m_z due to its inherent limitation of depth information deficiency. In order to solve this problem, we can use stereo cameras or a monocular camera with height information to obtain m_z.

2.3. Magnetic Sensor

A first-order Gauss-Markov vector random process with statistically independent components is chosen to model magnetic variations as follows [10]:

$${\dot{\mathbf{b}}}_h=-\alpha {\mathbf{b}}_h+{\mathbf{n}}_{\omega h}$$

(11)

where α is a positive constant, and n_ωh is white Gaussian noise.

3. MKF Algorithm

3.1. Process Model

The evolving state is described as follows:

$${\mathbf{\text{X}}}_k=\left[\begin{array}{cccccc}{\mathbf{\text{C}}}_{nk}^{\text{b}}& {\mathbf{\text{b}}}_{gk}& {\mathbf{\text{v}}}_k^n& {\mathbf{\text{m}}}_{\mathbf{v}k}^n& {\mathbf{\text{m}}}_{hk}^n& {\mathbf{b}}_{hk}\end{array}\right]$$

(12)

where mⁿ is the relative distance of the vehicle with respect to the observed line, vⁿ is the relative velocity.

The relative position of the vehicle with respect to the observed line, ξⁿ, can be calculated as follows:

• Calculate the point of intersection of the observed lines. According to Equation (6), we obtain:

  $${\mathbf{\xi }}_i^n=-{\left({\left[{\mathbf{\text{l}}}_v^n\times \right]}^2+{\left[{\mathbf{\text{l}}}_h^n\times \right]}^2\right)}^{-1}\left(\left[{\mathbf{\text{l}}}_v^n\times \right]{\text{m}}_v^n+\left[{\mathbf{\text{l}}}_h^n\times \right]{\text{m}}_h^n\right)$$

  (13)

• Calculate the translation along the observed line direction from the intersection to the vertical point. According to Equation (8), we obtain:

  $$\begin{array}{c}\Vert {\mathbf{\xi }}_{\perp v}^n\Vert =\Vert {\mathbf{\xi }}_i^n+{\rho }_v{\mathbf{\text{l}}}_v^n\Vert =\Vert {\mathbf{\text{m}}}_v^n\Vert \\ \Vert {\mathbf{\xi }}_{\perp h}^n\Vert =\Vert {\mathbf{\xi }}_i^n+{\rho }_h{\mathbf{\text{l}}}_h^n\Vert =\Vert {\mathbf{\text{m}}}_h^n\Vert \end{array}$$

  (14)

  Because the unique of perpendicular point, the translation along the observed line direction, ρ, is also uniqueness. From Equation (14), we obtain:

  $$\begin{array}{cc}{\rho }_v=-{\mathbf{\xi }}_i^{nT}{\mathbf{l}}_v^n& {\rho }_h=-{\mathbf{\xi }}_i^{nT}{\mathbf{l}}_h^n\end{array}$$

  (15)

• Calculate the relative position:

  $$\begin{array}{c}{\mathbf{\xi }}_{\perp v}^n={\mathbf{\xi }}_i^n+{\rho }_v{\mathbf{\text{l}}}_v^n={\mathbf{\xi }}_i^n-\left({\mathbf{\xi }}_i^{nT}{\mathbf{\text{l}}}_v^n\right){\mathbf{\text{l}}}_v^n\\ {\mathbf{\xi }}_{\perp h}^n={\mathbf{\xi }}_i^n+{\rho }_h{\mathbf{\text{l}}}_h^n={\mathbf{\xi }}_i^n-\left({\mathbf{\xi }}_i^{nT}{\mathbf{\text{l}}}_h^n\right){\mathbf{\text{l}}}_h^n\end{array}$$

  (16)

The discretization of Equation (3) gives the following equation [11]:

$${\mathbf{\text{C}}}_{nk+1}^{\text{b}}={\mathrm{\Phi }}_k{\mathbf{\text{C}}}_{nk}^b+[{\mathbf{b}}_g\times ]{\mathbf{\text{C}}}_{nk}^b\mathrm{\Delta }t+{\mathbf{\text{w}}}_k$$

(17)

where Φ_k is the transition matrix from time tk to time tk₊₁ that corresponds to $-[{\mathbf{\omega }}_{ib}^b\times ]$. We assume that ${\mathbf{\omega }}_{ib}^b$ is piece wisely constant in the tiny time intervals Δt = tk₊₁ − tk, and then, Φ_k in Equation (17) can be approximated as:

$${\mathrm{\Phi }}_k\approx {exp}^{-[{\mathbf{\omega }}_{ib}^b\times ]\mathrm{\Delta }t}$$

(18)

Furthermore, w_k can be written as:

$${\mathbf{\text{w}}}_k[{\mathbf{\text{n}}}_g\times ]{\mathbf{\text{C}}}_{nk}^b\mathrm{\Delta }t+h.o.t$$

(19)

where h.o.t denotes the terms of the second order Δt² and higher. Note that the process noise matrix, w_k, is state dependent, and that the first-order term is linear in the components of the white Gaussian noise vectors n_g.

Discretizing Equations (2), (4), (9), (11) produces:

$${\mathbf{\text{b}}}_{gk+1}={\mathbf{\text{b}}}_{gk}+{\mathbf{\text{n}}}_{\mathbf{\omega }g}\mathrm{\Delta }t$$

(20)

$${\mathbf{\text{v}}}_{k+1}^n={\mathbf{\text{v}}}_k^n+{\mathbf{\text{C}}}_b^n{\mathbf{\text{f}}}^b\mathrm{\Delta }t+{\mathbf{\text{g}}}^n\mathrm{\Delta }t$$

(21)

$${\mathbf{\text{m}}}_{k+1}^n={\mathbf{\text{m}}}_k^n+[{\mathbf{\text{l}}}^n\times ]{\mathbf{\text{v}}}^n\mathrm{\Delta }t$$

(22)

$${\mathbf{\text{b}}}_{hk+1}={exp}^{-\alpha \mathrm{\Delta }t}{\mathbf{\text{b}}}_{hk}+{\mathbf{\text{n}}}_{\mathbf{\omega }h}\mathrm{\Delta }t$$

(23)

Combining Equations (17), (20)–(23) together, we obtain the dynamic model as:

$${\mathbf{\text{X}}}_{k+1}=\sum _{r=1}^8{\mathrm{\Theta }}_k^r{\mathbf{\text{X}}}_k{\mathrm{\Lambda }}_k^r+{\mathbf{\text{B}}}_k{\mathbf{\text{U}}}_k\mathbf{\text{E}}+{\mathbf{\text{W}}}_k$$

(24)

where the dynamic matrices, ${\mathrm{\Theta }}_k^r$, ${\mathrm{\Lambda }}_k^r$, are defined as:

$$\begin{array}{ll}{\mathrm{\Theta }}_k^1={\mathrm{\Phi }}_k\hfill & {\mathrm{\Lambda }}_k^1={\mathbf{\text{E}}}^{11}+{\mathbf{\text{E}}}^{22}+{\mathbf{\text{E}}}^{23}\hfill \\ {\mathrm{\Theta }}_k^2=[{\mathbf{\text{c}}}_{1k}\times ]\hfill & {\mathrm{\Lambda }}_k^2={\mathbf{\text{E}}}^{41}\mathrm{\Delta }t\hfill \\ {\mathrm{\Theta }}_k^3=[{\mathbf{\text{c}}}_{2k}\times ]\hfill & {\mathrm{\Lambda }}_k^3={\mathbf{\text{E}}}^{42}\mathrm{\Delta }t\hfill \\ {\mathrm{\Theta }}_k^4=[{\mathbf{\text{c}}}_{3k}\times ]\hfill & {\mathrm{\Lambda }}_k^4={\mathbf{\text{E}}}^{43}\mathrm{\Delta }t\hfill \\ {\mathrm{\Theta }}_k^5={\mathbf{\text{I}}}_3\hfill & {\mathrm{\Lambda }}_k^5={\mathbf{\text{E}}}^{44}+{\mathbf{\text{E}}}^{55}+{\mathbf{\text{E}}}^{66}+{\mathbf{\text{E}}}^{77}\hfill \\ {\mathrm{\Theta }}_k^6=[{\mathbf{\text{I}}}_v^n\times ]\hfill & {\mathrm{\Lambda }}_k^6={\mathbf{\text{E}}}^{56}\mathrm{\Delta }t\hfill \\ {\mathrm{\Theta }}_k^7=[{\mathbf{\text{I}}}_h^n\times ]\hfill & {\mathrm{\Lambda }}_k^7={\mathbf{\text{E}}}^{57}\mathrm{\Delta }t\hfill \\ {\mathrm{\Theta }}_k^8={exp}^{-\alpha \mathrm{\Delta }t}{\mathbf{\text{I}}}_3\hfill & {\mathrm{\Lambda }}_k^8={\mathbf{\text{E}}}^{88}\hfill \end{array}$$

(25)

where E^ij denotes a 8 × 8 matrix with 1 at position (ij) and 0 elsewhere;

$$\mathbf{\text{B}}=[\begin{array}{cc}{\mathbf{\text{C}}}_{nk}^{bT}& {\mathbf{\text{I}}}_3\end{array}],{\mathbf{\text{U}}}_k=\left[\begin{array}{c}{\mathbf{\text{f}}}_k^b\\ {\mathbf{\text{g}}}^n\end{array}\right],\mathbf{\text{E}}=\left[\begin{array}{cc}{\mathbf{\text{e}}}_5^T\mathrm{\Delta }t& {\mathbf{0}}_{3\times 1}\end{array}\right]$$

(26)

and the noise matrix, W_k, is defined as:

$${\mathbf{\text{W}}}_k=[\begin{array}{llllll}{\mathbf{\text{w}}}_k\hfill & {\mathbf{\text{n}}}_{\mathbf{\omega }g}\mathrm{\Delta }t\hfill & {\mathbf{\text{n}}}_a\mathrm{\Delta }t\hfill & {\mathbf{\text{n}}}_c\mathrm{\Delta }t\hfill & {\mathbf{\text{n}}}_c\mathrm{\Delta }t\hfill & {\mathbf{\text{n}}}_h\mathrm{\Delta }t\hfill \end{array}]$$

(27)

The process noise covariance matrix is:

$${\mathbf{\text{Q}}}_k=\left[\begin{array}{cccccc}cov[{\text{w}}_k]& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}\\ {\mathbf{0}}_{3\times 9}& {\sigma }_{\mathbf{\omega }g}^2\mathrm{\Delta }t{\mathbf{\text{I}}}_3& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 9}& {\mathbf{0}}_{3\times 3}& {\sigma }_a^2\mathrm{\Delta }t{\mathbf{\text{I}}}_3& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 9}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\sigma }_c^2\mathrm{\Delta }t{\mathbf{\text{I}}}_3& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 9}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\sigma }_c^2\mathrm{\Delta }t{\mathbf{\text{I}}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 9}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\sigma }_h^2\frac{1-{exp}^{-2\alpha \mathrm{\Delta }t}}{2\alpha }{\mathbf{\text{I}}}_3\end{array}\right]$$

(28)

where, as defined by [11]:

$$cov[{\mathbf{\text{w}}}_k]=\left({\left({\widehat{\mathbf{C}}}_{nk}^b\right)}^T\otimes {\mathbf{\text{I}}}_3\right)\mathbf{\text{L}}{\sigma }_g^2{\mathbf{\text{I}}}_3{\mathbf{\text{L}}}^T\left({\widehat{\mathbf{C}}}_{nk}^b\otimes {\mathbf{\text{I}}}_3\right)\mathrm{\Delta }{t}^2$$

(29)

L is the 9 × 3 matrix defined as:

$${\mathbf{\text{L}}}^T=\left[\begin{array}{ccc}[{\mathbf{\text{e}}}_1\times ]& [{\mathbf{\text{e}}}_2\times ]& [{\mathbf{\text{e}}}_3\times ]\end{array}\right]$$

(30)

with e₁ = [1 0 0]^T, e₂ = [0 1 0]^T, and e₃ = [0 0 1]^T.

The process Equation (24) is not linear because in the matrix ${\mathrm{\Theta }}_k^r$, r = 2, 3, 4, are the functions of elements of ${\mathbf{\text{C}}}_{nk}^b$. One way of overcoming this difficulty is to substitute ${\widehat{\mathbf{C}}}_{nk/k}^b$ for ${\mathbf{\text{C}}}_{nk}^b$ in the process equation. This substitution yields a pseudo-linear process equation.

3.2. Measurement Model

The matrix measurement equation:

$${\mathbf{\text{Y}}}_{k+1}=\left[\begin{array}{cc}{\mathbf{\text{m}}}_{k+1}^c& {\mathbf{\text{m}}}_{mk+1}\end{array}\right]=\sum _{s=1}^2{\mathbf{\text{H}}}_{k+1}^s{\mathbf{\text{X}}}_{k+1}{\mathbf{\text{G}}}_{k+1}^s+{\mathbf{\text{V}}}_{k+1}$$

(31)

where the measurement matrices are:

$$\begin{array}{c}{\mathbf{\text{H}}}_{k+1}^1={\mathbf{\text{C}}}_b^c,{\mathbf{\text{G}}}_{k+1}^1=\left[\begin{array}{cc}{\mathbf{\text{m}}}_k^n& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{5\times 1}& {\mathbf{0}}_{5\times 1}\end{array}\right]\\ {\mathbf{\text{H}}}_{k+1}^2={\mathbf{\text{I}}}_3,{\mathbf{\text{G}}}_{k+1}^2=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 1}& \mathbf{\text{h}}+b_h\\ {\mathbf{0}}_{5\times 1}& {\mathbf{0}}_{5\times 1}\end{array}\right]\end{array}$$

(32)

and the measurement noise covariance matrix is given as:

$${\mathbf{\text{R}}}_{k+1}=\left[\begin{array}{cc}{\sigma }_c^2{\mathbf{\text{I}}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\sigma }_h^2{\mathbf{\text{I}}}_3\end{array}\right]$$

(33)

3.3. Update

Putting m = 3, n = 8, p = 3, q = 2, μ= 8, v = 2 into Equations described in [5], we can obtain time update and measurement update equations. However, due to the existence of the input matrix, the time update equation below is not the same as that given in [15]:

$${\widehat{\mathbf{X}}}_{k+1/k}=\sum _{r=1}^8{\mathrm{\Theta }}_k^r{\widehat{\mathbf{X}}}_{k/k}{\mathrm{\Lambda }}_k^r+{\mathbf{\text{B}}}_k{\mathbf{\text{U}}}_k\mathbf{\text{E}}$$

(34)

$${\mathrm{\Phi }}_k=\sum _{r=1}^8\left[{\left({\mathrm{\Lambda }}_k^r\right)}^T\otimes {\mathrm{\Theta }}_k^r\right]$$

(35)

$${\mathbf{\text{P}}}_{k+1/k}={\mathrm{\Phi }}_k{\mathbf{\text{P}}}_{k/k}{\mathrm{\Phi }}_k^T+{\mathbf{\text{Q}}}_k$$

(36)

Measurement update equations:

$${\widehat{\mathbf{\text{Y}}}}_{k+1}=\sum _{s=1}^2{\mathbf{\text{H}}}_{k+1}^s{\widehat{\mathbf{X}}}_{k+1/k}{\mathbf{\text{G}}}_{k+1}^s-{\mathbf{Y}}_{k+1}$$

(37)

$${\mathbf{\text{H}}}_{k+1}=\sum _{s=1}^2\left[{\left({\mathbf{\text{G}}}_{k+1}^s\right)}^T\otimes {\mathbf{\text{H}}}_{k+1}^s\right]$$

(38)

$${\mathbf{\text{S}}}_{k+1}={\mathbf{\text{H}}}_{k+1}{\mathbf{\text{P}}}_{k+1/k}{\mathbf{\text{H}}}_{k+1}^T+{\mathbf{\text{R}}}_{k+1}$$

(39)

$${\mathbf{\text{K}}}_{k+1}={\mathbf{\text{P}}}_{k+1}{\mathbf{\text{H}}}_{k+1}^T{\mathbf{\text{S}}}_{k+1}^{-1}$$

(40)

$${\widehat{\mathbf{X}}}_{k+1/k+1}={\widehat{\mathbf{X}}}_{k+1/k}-\sum _{j=1}^8\sum _{l=1}^2{\mathbf{\text{K}}}_{k+1}^{jl}{\tilde{\mathbf{Y}}}_{k+1}{\mathbf{\text{E}}}^{lj}$$

(41)

where ⊗ denotes the Kronecker product [12], ${\mathbf{\text{K}}}_{k+1}^{jl}$ is a 3 × 3 submatrix of the 24 × 6 matrix K_k+1 defined by:

$${\mathbf{\text{K}}}_{k+1}=\left[\begin{array}{cc}{\mathbf{\text{K}}}_{k+1}^{11}& {\mathbf{\text{K}}}_{k+1}^{12}\\ ⋮& ⋮\\ {\mathbf{\text{K}}}_{k+1}^{81}& {\mathbf{\text{K}}}_{k+1}^{82}\end{array}\right]$$

(42)

and E^lj is a 2 × 8 matrix with 1 at position (ij) and 0 elsewhere:

$${\mathbf{P}}_{k+1/k+1}=({\mathbf{I}}_{24}-\mathbf{K}\mathbf{\text{H}}){\mathbf{P}}_{k+1/k}{({\mathbf{I}}_{24}-\mathbf{K}\mathbf{\text{H}})}^T+\mathbf{K}\mathbf{R}{\mathbf{K}}^T$$

(43)

3.4. Orthogonalization

The Kalman filter is not designed to preserve any relationship among the components of the estimated state matrix ${\widehat{\mathbf{C}}}_{nk/k}^b$. It would not preserve the orthogonality. Iterative brute-force orthogonalization is presented for its low computation load [11]. The iterative brute-force procedure is as follows [13]:

$${\mathbf{\text{C}}}_n^{b\ast }={\widehat{\mathbf{C}}}_n^b\left(1.5{\mathbf{\text{I}}}_3-0.5{\widehat{\mathbf{C}}}_n^{bT}{\widehat{\mathbf{C}}}_n^b\right)$$

(44)

Although this algorithm is suboptimal, it is much simpler than the optimal algorithm given by Equation (45). When applied recursively, this algorithm produces a sequence of estimates that converge to the optimal solution of Equation (45) [14]. Moreover, as evidenced by simulations, orthogonality can be usually reached in all practical applications after just one or two iterations of Equation (44):

$${\mathbf{\text{C}}}_n^{b\ast }={\left({\widehat{\mathbf{C}}}_n^b{\widehat{\mathbf{C}}}_n^{bT}\right)}^{-\frac{1}{2}}{\widehat{\mathbf{C}}}_n^b$$

(45)

4. Observability Analysis

A system is observable if its states at a certain time instant can be uniquely determined given a finite sequence of its outputs [15]. Intuitively, observability means that the measurements of an observable system can provide sufficient geometric information for estimating its states. In contrast, the state vector of an unobservable system cannot be recovered regardless of the duration of the estimation process.

In this paper, we study the observability of the nonlinear system describing the visual/inertial/magnetic sensor based navigation process. The system state vector is chosen as follows:

$$\mathbf{\text{x}}(t)={\left[\begin{array}{cccccc}{\mathbf{\text{c}}}^T& {\mathbf{\text{b}}}_g^T& {\mathbf{\text{v}}}^{nT}& {\mathbf{\text{m}}}_v^{nT}& {\mathbf{\text{m}}}_h^{nT}& {\mathbf{\text{b}}}_n^T\end{array}\right]}^T$$

(46)

where c is defined as:

$$\begin{array}{cc}\mathbf{\text{c}}=\left[\begin{array}{c}{\mathbf{\text{c}}}_1\\ {\mathbf{\text{c}}}_2\\ {\mathbf{\text{c}}}_3\end{array}\right],& {\mathbf{\text{C}}}_n^b=[\begin{array}{ccc}{\mathbf{\text{c}}}_1& {\mathbf{\text{c}}}_2& {\mathbf{\text{c}}}_3\end{array}]\end{array}$$

(47)

Equation (3) can be rewritten as:

$$\dot{\mathbf{\text{c}}}=\mathrm{\Xi }\left({\mathbf{\text{b}}}_g\right)\mathbf{\text{c}}-\mathrm{\Xi }\left({\mathbf{\omega }}_{i\mathbf{\text{b}}}^b\right)\mathbf{\text{c}}=\mathrm{\Xi }\left({\mathbf{b}}_g\right)\mathbf{c}+\text{T}{\mathbf{\omega }}_{ib}^b$$

(48)

For arbitrary 3 × 1 vector p, we define:

$$\mathrm{\Xi }(\mathbf{\text{p}})=\left[\begin{array}{ccc}[\mathbf{\text{p}}\times ]& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& [\mathbf{\text{p}}\times ]& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& [\mathbf{\text{p}}\times ]\end{array}\right],\text{T}=\left[\begin{array}{c}[{\mathbf{\text{c}}}_1\times ]\\ [{\mathbf{\text{c}}}_2\times ]\\ [{\mathbf{\text{c}}}_3\times ]\end{array}\right]$$

(49)

Then, we rearrange the nonlinear kinematic Equations (2), (4), (9), (11), (48) in a pseudo-linear format for computing the Lie derivatives:

$$\left[\begin{array}{c}\dot{\mathbf{\text{c}}}\\ {\dot{\mathbf{\text{b}}}}_{\mathbf{\text{g}}}\\ {\dot{\mathbf{\text{v}}}}_g\\ {\dot{\mathbf{m}}}_{\mathbf{\text{v}}}^n\\ {\dot{\mathbf{m}}}_h^n\\ {\dot{\mathbf{\text{b}}}}_h\end{array}\right]=\underset{{\mathbf{\text{f}}}_0}{\underbrace{\left[\begin{array}{c}\mathrm{\Xi }({\mathbf{b}}_g)\mathbf{c}\\ {\mathbf{0}}_{3\times 1}\\ {\mathbf{g}}^n\\ \left[{\mathbf{\text{I}}}_v^n\times \right]{\mathbf{v}}^n\\ \left[{\mathbf{\text{I}}}_h^n\times \right]{\mathbf{v}}^n\\ -\alpha {\mathbf{b}}_h\end{array}\right]}}+\underset{\underset{\_}{{\mathbf{\text{f}}}_1}}{\underbrace{\left[\begin{array}{c}\mathbf{\text{T}}\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\end{array}\right]}}{\mathbf{\omega }}_{ib}^b+\underset{\underset{\_}{{\mathbf{\text{f}}}_2}}{\underbrace{\left[\begin{array}{c}{\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{\text{C}}}_b^n\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}\end{array}\right]}}{\mathbf{\text{f}}}^b$$

(50)

where ${\mathbf{\omega }}_{ib}^b$ and f^b are the gyro and accelerometer measurements.

Also, note that f₀ is a 24 × 1 vector, while f̠₁ and f̠₂ are both compact representations of three vectors of dimension 24 × 1, i.e.,

$$\underset{\_}{{\mathbf{\text{f}}}_1}{\mathbf{\omega }}_{ib}^b={\text{f}}_{11}{\omega }_x+{\text{f}}_{12}{\omega }_y+{\text{f}}_{13}{\omega }_z$$

(51)

where, for i = 1,2,3, f_li denotes the i th column vector comprising f̠₁ and ω_x, ω_y, and ω_z are the scalar components of the rotational velocity vector.

The measurement functions are as follows:

$${\mathbf{\text{h}}}^m={\mathbf{\text{h}}}_1^{\ast }(\mathbf{\text{x}})={\mathbf{\text{C}}}_n^b(\mathbf{\text{h}}+{\mathbf{\text{b}}}_{\mathbf{\text{h}}})=\mathrm{\Psi }(\mathbf{h}+{\mathbf{b}}_h)\mathbf{\text{c}}$$

(52)

$${\mathbf{\text{m}}}_n^{\mathbf{\text{c}}}={\mathbf{\text{h}}}_2^{\ast }(\mathbf{\text{x}})={\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_v^n={\mathbf{\text{C}}}_b^c\mathrm{\Psi }({\mathbf{\text{m}}}_v^n)\mathbf{c}$$

(53)

$${\mathbf{\text{m}}}_{\mathbf{\text{h}}}^{\mathbf{\text{c}}}=h_3^{\ast }(\mathbf{\text{x}})={\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_h^n={\mathbf{\text{C}}}_b^c\mathrm{\Psi }({\mathbf{\text{m}}}_h^n)\mathbf{c}$$

(54)

where:

$$\begin{array}{cc}{\mathbf{\text{C}}}_n^b\mathbf{\text{p}}=\mathrm{\Psi }(\mathbf{\text{p}})\mathbf{\text{c}},& \mathbf{\text{p}}={[\begin{array}{ccc}{p}_1& p_2& p_3\end{array}]}^T\end{array}$$

(55)

$$\mathrm{\Psi }(\mathbf{\text{p}})=[\begin{array}{ccc}{p}_1{\mathbf{\text{I}}}_3& p_2{\mathbf{\text{I}}}_3& p_3{\mathbf{\text{I}}}_3\end{array}]$$

(56)

Furthermore, we enforce the orthogonal constraints by employing the following additional measurement equations:

$${\mathbf{h}}_4^{\ast }(\mathbf{x})=\left[\begin{array}{c}{{\mathbf{c}}_1}^T{\mathbf{c}}_1\\ {{\mathbf{c}}_2}^T{\mathbf{c}}_2\\ {{\mathbf{c}}_3}^T{\mathbf{c}}_3\\ {c_{11}}^2+{c_{12}}^2+{c_{13}}^2\\ {c_{21}}^2+{c_{22}}^2+{c_{23}}^2\\ {c_{31}}^2+{c_{32}}^2+{c_{33}}^2\end{array}\right]-\left[\begin{array}{c}1\\ 1\\ 1\\ 1\\ 1\\ 1\end{array}\right]=0$$

(57)

It suffices to show that a subset of the rows of the observability matrix O (cf. Equation (75)) are linearly independent. In the remainder of this section, we will prove that the system described by Equations (50)–(57) is observable by computing among the candidate the zeroth- and first- order Lie derivatives of ${\mathbf{\text{h}}}_1^{\ast }$, ${\mathbf{\text{h}}}_2^{\ast }$, ${\mathbf{\text{h}}}_3^{\ast }$, and ${\mathbf{\text{h}}}_4^{\ast }$, the ones whose gradients ensure that the observability matrix O is full rank.

• The zeroth-Order Lie Derivatives $\left({𝔏}^0{\mathbf{\text{h}}}_1^{\ast },{𝔏}^0{\mathbf{\text{h}}}_2^{\ast },{𝔏}^0{\mathbf{\text{h}}}_3^{\ast },{𝔏}^0{\mathbf{\text{h}}}_4^{\ast }\right)$: By definition, the zeroth-order Lie derivative of a function is the function itself, i.e.,

  $${𝔏}^0{\mathbf{\text{h}}}_1^{\ast }={\mathbf{\text{h}}}_1^{\ast }={\mathbf{\text{C}}}_n^b(\mathbf{\text{h}}+{\mathbf{\text{b}}}_h)=\mathrm{\Psi }(h+b_h)\mathbf{\text{c}}$$

  (58)
  $${𝔏}^0{\mathbf{\text{h}}}_2^{\ast }={\mathbf{\text{h}}}_2^{\ast }={\mathbf{\text{C}}}_b^{\mathbf{\text{c}}}{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_v^n={\mathbf{\text{C}}}_b^c\mathrm{\Psi }({\mathbf{\text{m}}}_v^n)\mathbf{\text{c}}$$

  (59)
  $${𝔏}^0{\mathbf{\text{h}}}_3^{\ast }={\mathbf{\text{h}}}_3^{\ast }={\mathbf{\text{C}}}_b^{\mathbf{\text{c}}}{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_h^n={\mathbf{\text{C}}}_b^c\mathrm{\Psi }({\mathbf{\text{m}}}_h^n)\mathbf{\text{c}}$$

  (60)
  $${𝔏}^0{\mathbf{\text{h}}}_4^{\ast }={\mathbf{\text{h}}}_4^{\ast }=\left[\begin{array}{c}{{\mathbf{\text{c}}}_1}^T{\mathbf{\text{c}}}_1\\ {{\mathbf{\text{c}}}_2}^T{\mathbf{\text{c}}}_2\\ {{\mathbf{\text{c}}}_3}^T{\mathbf{\text{c}}}_3\\ {c_{11}}^2+{c_{12}}^2+{c_{13}}^2\\ {c_{21}}^2+{c_{22}}^2+{c_{23}}^2\\ {c_{31}}^2+{c_{32}}^2+{c_{33}}^2\end{array}\right]-\left[\begin{array}{c}1\\ 1\\ 1\\ 1\\ 1\\ 1\end{array}\right]$$

  (61)

  Therefore, the gradients of the zeroth-order Lie derivatives are exactly the same as the Jacobians of the corresponding measurement functions:

  $$\nabla {𝔏}^0{\mathbf{\text{h}}}_1^{\ast }=\left[\begin{array}{ccc}\mathrm{\Psi }(\mathbf{\text{h}}+{\mathbf{\text{b}}}_h)& {\mathbf{0}}_{3\times 12}& {\mathbf{\text{C}}}_n^b\end{array}\right]$$

  (62)
  $$\nabla {𝔏}^0{\mathbf{h}}_2^{\ast }=\left[\begin{array}{cccc}{\mathbf{C}}_b^c\mathrm{\Psi }({\mathbf{m}}_v^n)& {\mathbf{0}}_{3\times 6}& {\mathbf{C}}_b^c{\mathbf{C}}_n^b& {\mathbf{0}}_{3\times 6}\end{array}\right]$$

  (63)
  $$\nabla {𝔏}^0{\mathbf{\text{h}}}_3^{\ast }=\left[\begin{array}{cccc}{\mathbf{\text{C}}}_b^c\mathrm{\Psi }({\mathbf{\text{m}}}_h^n)& {\mathbf{0}}_{3\times 9}& {\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b& {\mathbf{0}}_{3\times 3}\end{array}\right]$$

  (64)
  $$\nabla {𝔏}^0{\mathbf{\text{h}}}_4^{\ast }=[\begin{array}{cc}2\mathbf{\text{M}}& {\mathbf{0}}_{6\times 15}\end{array}]$$

  (65)
  where:
  $$\mathbf{\text{M}}=\left[\begin{array}{ccccccccc}{c}_{11}& c_{21}& c_{31}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& c_{12}& c_{22}& c_{32}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& c_{13}& c_{23}& c_{33}\\ c_{11}& 0& 0& c_{12}& 0& 0& c_{13}& 0& 0\\ 0& c_{21}& 0& 0& c_{22}& 0& 0& c_{23}& 0\\ 0& 0& c_{31}& 0& 0& c_{32}& 0& 0& c_{33}\end{array}\right]$$

  (66)

• The first-Order Lie Derivatives $\left(\nabla {𝔏}_{{\text{f}}_0}^1{\mathbf{\text{h}}}_2^{\ast },\nabla {𝔏}_{{\text{f}}_0}^1{\mathbf{\text{h}}}_3^{\ast },\nabla {𝔏}_{\underset{\_}{{\text{f}}_1}}^1{\mathbf{\text{h}}}_1^{\ast },\right)$: The first-order Lie derivatives of ${\mathbf{h}}_2^{\ast }$ and ${\mathbf{\text{h}}}_3^{\ast }$ with respect to f₀ are computed as:

  $$\begin{array}{c}\nabla {𝔏}_{{\text{f}}_0}^1{\mathbf{\text{h}}}_2^{\ast }=\nabla {𝔏}^0{\mathbf{\text{h}}}_2^{\ast }\cdot {\text{f}}_0={\mathbf{\text{C}}}_b^c\mathrm{\Psi }\left({\mathbf{\text{m}}}_v^n\right)\mathrm{\Xi }\left({\mathbf{\text{b}}}_g\right)\mathbf{c}+{\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b\left[{\mathbf{\text{l}}}_{\mathbf{\text{v}}}^n\times \right]{\mathbf{v}}^n\\ ={\mathbf{\text{C}}}_b^c\left[{\mathbf{b}}_g\times \right]{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_v^n+{\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b\left[{\mathbf{\text{l}}}_v^n\times \right]{\mathbf{v}}^n\end{array}$$

  (67)
  $$\nabla {𝔏}_{{\text{f}}_0}^1{\mathbf{\text{h}}}_3^{\ast }=\nabla {𝔏}^0{\mathbf{\text{h}}}_3^{\ast }·{\text{f}}_0={\mathbf{\text{C}}}_b^c\left[{\mathbf{b}}_g\times \right]{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_h^n+{\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b\left[{\mathbf{\text{l}}}_h^n\times \right]{\mathbf{\text{v}}}^n$$

  (68)

Equation (67) is proved in Appendix A, while their gradients are given by:

$$\nabla {𝔏}_{{\mathbf{\text{f}}}_0}^1{\mathbf{\text{h}}}_2^{\ast }=\left[\begin{array}{ccccc}{\mathbf{\text{X}}}_1& -{\mathbf{\text{C}}}_b^c\left[{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_v^n\times \right]& {\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b\left[{\mathbf{\text{l}}}_v^n\times \right]& {\mathbf{\text{C}}}_b^c\left[b_g\times \right]{\mathbf{\text{C}}}_n^b& {\mathbf{0}}_{3\times 6}\end{array}\right]$$

(69)

$$\nabla {𝔏}_{{\text{f}}_0}^1{\mathbf{\text{h}}}_3^{\ast }=\left[\begin{array}{cccccc}{\mathbf{\text{X}}}_1& -{\mathbf{\text{C}}}_b^c\left[{\mathbf{\text{C}}}_n^b{\mathbf{\text{m}}}_h^n\times \right]& {\mathbf{\text{C}}}_b^c{\mathbf{\text{C}}}_n^b\left[{\mathbf{\text{l}}}_h^n\times \right]& {\mathbf{0}}_{3\times 3}& {\mathbf{\text{C}}}_b^c\left[{\mathbf{b}}_g\times \right]{\mathbf{\text{C}}}_n^b& {\mathbf{0}}_{3\times 3}\end{array}\right]$$

(70)

In these expressions, X₁ and X₂ are both 3 × 9 matrices and regardless of their values, they will be eliminated from the following derivations; hence, they need not be computed explicitly.

The next first-order Lie derivative of interest is that of ${\mathbf{\text{h}}}_1^{\ast }$ with respect to f̠₁, i.e., $\nabla {𝔏}_{\underset{\_}{{\text{f}}_1}}^1{\mathbf{\text{h}}}_1^{\ast }$. At this point, it is important to note that f̠₁ as defined in Equation (50) is a compact representation of three column vectors. Similarly, we can also write the resultant Lie derivative in a compact form (i.e., a 3 × 3 matrix):

$$\nabla {𝔏}_{\underset{\_}{{\text{f}}_1}}^1{\mathbf{h}}_1^{\ast }=\nabla {𝔏}^0{\mathbf{h}}_1^{\ast }\cdot \underset{\_}{{\text{f}}_1}=\mathrm{\Psi }(\mathbf{h}+{\mathbf{b}}_h)\mathbf{\text{T}}$$

(71)

Stacking the gradients of the three columns together gives:

$$\nabla {𝔏}_{\underset{\_}{{\text{f}}_1}}^1{\mathbf{\text{h}}}_1^{\ast }=[\begin{array}{ccc}\mathbf{\Gamma }& {\mathbf{0}}_{9\times 12}& \mathbf{\gamma }\end{array}]$$

(72)

where the matrices:

$$\mathbf{\Gamma }=\left[\begin{array}{c}{\mathbf{\Gamma }}_1\\ {\mathbf{\Gamma }}_2\\ {\mathbf{\Gamma }}_3\end{array}\right],\mathbf{\gamma }=\left[\begin{array}{c}{\mathbf{\gamma }}_1\\ {\mathbf{\gamma }}_2\\ {\mathbf{\gamma }}_3\end{array}\right]$$

(73)

of dimensions 9 × 9 and 9 × 3, have the block-row elements (for i = 1,2,3) as follows:

$${\mathbf{\Gamma }}_i=\frac{\partial }{\partial \mathbf{\text{c}}}[\mathrm{\Psi }(\mathbf{\text{h}}+{\mathbf{\text{b}}}_h)\mathbf{\text{T}}{\mathbf{\text{e}}}_i],{\mathbf{\gamma }}_i=\frac{\partial }{\partial {\mathbf{\text{b}}}_h}[\mathrm{\Psi }(\mathbf{h}+{\mathbf{\text{b}}}_h)\mathbf{\text{T}}{\mathbf{\text{e}}}_i]$$

(74)

Stacking together all the previously computed gradients of the Lie derivatives, we form the observability matrix O as:

$$𝒪=\left[\begin{array}{c}\nabla {𝔏}^0{\mathbf{h}}_1^{\ast }\\ \nabla {𝔏}^0{\mathbf{h}}_2^{\ast }\\ \nabla {𝔏}^0{\mathbf{h}}_3^{\ast }\\ \nabla {𝔏}_{{\mathbf{\text{f}}}_0}^1{\mathbf{h}}_2^{\ast }\\ \nabla {𝔏}_{{\mathbf{\text{f}}}_0}^1{\mathbf{h}}_3^{\ast }\\ \nabla {𝔏}_{\underset{\_}{{\mathbf{\text{f}}}_1}}^1{\mathbf{h}}_1^{\ast }\\ \nabla {𝔏}^0{\mathbf{h}}_4^{\ast }\end{array}\right]=\left[\begin{array}{cccccc}\mathrm{\Psi }(\mathbf{h}+{\mathbf{b}}_h)& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{C}}_n^b\\ {\mathbf{C}}_b^c\mathrm{\Psi }({\mathbf{m}}_v^n)& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{C}}_b^c{\mathbf{C}}_n^b& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{C}}_b^c\mathrm{\Psi }({\mathbf{m}}_h^n)& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{C}}_b^c{\mathbf{C}}_n^b& {\mathbf{0}}_{3\times 3}\\ {\mathbf{X}}_1& -{\mathbf{C}}_b^c\left[{\mathbf{C}}_n^b{\mathbf{m}}_v^n\times \right]& {\mathbf{C}}_b^c{\mathbf{C}}_n^b\left[{\mathbf{l}}_v^n\times \right]& {\mathbf{C}}_b^c\left[{\mathbf{b}}_g\times \right]{\mathbf{C}}_n^b& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{X}}_2& -{\mathbf{C}}_b^c\left[{\mathbf{C}}_n^b{\mathbf{m}}_h^n\times \right]& {\mathbf{C}}_b^c{\mathbf{C}}_n^b\left[{\mathbf{l}}_h^n\times \right]& {\mathbf{0}}_{3\times 3}& {\mathbf{C}}_b^c\left[{\mathbf{b}}_g\times \right]{\mathbf{C}}_n^b& {\mathbf{0}}_{3\times 3}\\ \mathbf{\Gamma }& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& \mathbf{\gamma }\\ 2\mathbf{M}& {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}\end{array}\right]$$

(75)

In order to prove that the system described by Equations (50)–(57) is observable, we show that the matrix O is full rank (i.e., the state space of the system is spanned by the gradients of the Lie derivatives of the measurement functions [16,17]). Before presenting the main result of this section, we first present the following two lemmas whose proofs are detailed in Appendixs B and C, respectively.

• Lemma 1: The 15 × 12 matrix:

  $$\mathbf{\text{A}}=\left[\begin{array}{cc}\mathbf{\Gamma }& \mathbf{\gamma }\\ 2\mathbf{\text{M}}& {\mathbf{0}}_{6\times 3}\end{array}\right]$$

  (76)
  is rank 9 if at least one degree of rotational freedom is excited.

• Lemma 2: The 6 × 6 matrix:

  $$\mathbf{B}=\left[\begin{array}{cc}-{\mathbf{C}}_b^c\left[{\mathbf{C}}_n^b{\mathbf{m}}_v^n\times \right]& {\mathbf{C}}_b^c{\mathbf{C}}_n^b\left[{\mathbf{l}}_v^n\times \right]\\ -{\mathbf{C}}_b^c\left[{\mathbf{C}}_n^b{\mathbf{m}}_h^n\times \right]& {\mathbf{C}}_b^c{\mathbf{C}}_n^b\left[{\mathbf{l}}_h^n\times \right]\end{array}\right]$$

  (77)
  is full rank if two linearly independent horizontal lines are observed.

The observability matrix O (cf. Equation (75)) is full rank when: (a) at least one degree of rotational freedom is excited; (b) at least one vertical line and two linearly independent horizontal lines are observed. The proofs are given in Appendix D.

5. Experimental Results

5.1. Hardware Description

In order to demonstrate the validity of the proposed algorithm in realistic situations, we conducted indoor experiments using a testbed that consists of an ISIS IMU, a Firewire camera, a magnetometer, and a computer for data acquisition. The IMU, the camera, and the magnetometer were rigidly mounted on the chassis and their relative pose did not change during the experiment. The magnetometer was placed far from current wires, computer, and ferromagnetic materials. The raw data were delivered through an USB interface to the computer. The intrinsic parameters of the camera and transformation among the IMU, the camera and the magnetometer were calibrated prior to the experiment and were treated as constants. A monocular camera cannot determine the depth information. In order to solve the problem, the height of the camera from the ground plane was measured, and the lines which are on the ground and perpendicular to the ground were chosen as landmarks. We used basic trigonometry to determine the depth information. m^c can be calculated as measurements. Accuracies of the sensors are listed in

Table 1. Accuracies of the Sensors.

Sensors	Number	Accuracies	Sampling Rates
IMU	1	gyro drift: <0.01°/s (short term)	100 Hz
		accelerometer bias: ±2 mg (short term)
Camera	1	Resolution: 656 × 490	10 Hz
		focus length: 25 mm
		field of view: 60°
Magnetometer	1	σbh:10 u.a./s(×10−3)	100 Hz
		α:10 s−1
		σh:1 mGauss

. The IMU, camera and magnetometer were electronically time-synchronized.

5.2. Experiment Profile

A short distance experiment was carried out in the hallway. The testbed was rigidly mounted on the chassis of a pushcart. The estimated 3-D trajectory of the pushcart can be seen in Figure 2. The initial position of the pushcart is denoted by ‘*’.

The line features were extracted using the Hough transform. The line-points can be easily calculated with the detection values (ρ,θ) from the Hough transform. It was assumed that the camera measurements were corrupted by additive white Gaussian noise with a standard deviation of 2 pixels. The actual pixel noise is less than 2 pixels. However, in order to compensate the existence of the unmodeled nonlinearities and imperfect camera calibration, the noise standard deviation was increased to 2 pixels.

5.3. Algorithm Performance

The trajectory of the pushcart included two loops. The final position estimate, expressed with respect to the starting pose, is [0.81,0.59,0.06]^T m. From the initial and final parking spots of the pushcart, it is known that the true final position expressed with respect to the initial pose is approximately [0,0,0]^T m. Thus, the final position error is approximately 1 m at the end of a trajectory of 100 m, i.e., an error of 1% of the travelled distance.

It is noteworthy that the camera motion was almost in parallel to the optical axis, a condition which is particularly adverse for the image-based motion estimation algorithms [18]. In Figure 3, the 3σ bounds for the errors in the attitude matrix and the velocities along the three axes are shown. The plotted values are 3-times the square roots of the corresponding diagonal elements of the state covariance matrix. Position errors were mainly caused by the precision of visual measurement and attitude estimate, especially attitude estimate. Because the steel framed buildings affect the magnetic field, the precision of attitude estimate by inertial/magnetic sensors is not high.

6. Conclusions

In this paper, we have studied observability of a MKF-based algorithm for indoor navigation using visual/inertial/magnetic sensors. The estimation algorithm uses a compact matrix notation to produce the matrix estimate and the estimation error covariance matrix. The MKF is a natural and straightforward extension of the ordinary KF. Compared with the ordinary KF, the MKF model consists of both pseudo-linear process model and nonlinear measurement model.

The observability of the nonlinear system describing the integrated navigation with visual/inertial/magnetic sensors was investigated by employing the observability rank conditions defined with the Lie derivatives. The pseudo-linear process help simplifying the gradient operator in the process of observability analysis. This paper has for the first time proved that the observability matrix is full rank when: (a) at least one degree of rotational freedom is excited; (b) at least two linearly independent horizontal lines and one vertical line are observed. When these conditions are satisfied, the state matrix that contains the body attitude matrix, the gyro bias vector, relative velocity vector, the dual part of landmark and the magnetic variation superimposed to the magnetic reference vector are observable. The indoor experimental results have demonstrated the correctness of observability conditions.