Open AccessArticle

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

Guohu Feng

^1,*,

Wenqi Wu

¹ and

Jinling Wang

The College of Mechatronics and Automation, National University of Defense Technology, Changsha 410073, Hunan, China

School of Surveying and Spatial Information Systems, University of New South Wales, Sydney, NSW 2052, Australia

Author to whom correspondence should be addressed.

Sensors 2012, 12(7), 8877-8894; https://doi.org/10.3390/s120708877

Submission received: 14 May 2012 / Revised: 14 June 2012 / Accepted: 18 June 2012 / Published: 27 June 2012

(This article belongs to the Special Issue New Trends towards Automatic Vehicle Control and Perception Systems)

Download

Browse Figures

Versions Notes

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.

Keywords:

matrix Kalman filter; Lie derivatives; observability of nonlinear systems; navigation; vision; inertial measurement unit

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 $C_{n}^{b}$ satisfy the kinematic equations as [1,7]:

{\dot{ξ}}^{n} = - v^{n}

(1)

{\dot{v}}^{n} = C_{b}^{n} (f^{b} - b_{a}) + g^{n}

(2)

{\dot{C}}_{n}^{b} = - [ω_{n b}^{b} \times] C_{n}^{b}, ω_{n b}^{b} = ω_{i b}^{b} - b_{g}

(3)

{\dot{b}}_{g} = n_{ω 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.

[ω_{n b}^{b} \times]

is the skew symmetric matrix of

ω_{n b}^{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_{l p} = \frac{f m_{x}^{c} m_{z}^{c}}{m_{x}^{c 2} + m_{y}^{c 2}}, y_{l p} = \frac{f m_{y}^{c} m_{z}^{c}}{m_{x}^{c 2} + m_{y}^{c 2}}

(5)

Line features are represented by quaternion. Î = 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:

m^{n} = ξ^{n} \times l^{n}

(6)

while in the c-frame:

\begin{matrix} m^{c} & = (C_{n}^{c} (ξ^{n} - C_{n}^{c} r^{b})) \times (C_{n}^{c} l^{n}) = C_{n}^{c} ((ξ^{n} - C_{n}^{c} r^{b}) \times l^{n}) \\ = C_{n}^{c} m^{n} - C_{n}^{c} r^{b} \times l^{n} \approx C_{n}^{c} m^{n} \end{matrix}

(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:

‖ m^{n} ‖ = ‖ ξ^{n} ‖ ‖ l^{n} ‖ = ‖ ξ^{n} ‖

(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{m}}^{n} = ξ^{n} \times l^{n} = - v^{n} \times l^{n} = [l^{n} \times] v^{n}

(9)

For simplification, only vertical lines $l_{v}^{n}$ and horizontal lines $l_{h}^{n}$ are chosen as landmarks. According to Equation (5), we obtain:

m^{c} = {[\begin{matrix} m_{x} & m_{y} & m_{z} \end{matrix}]}^{T} = {[\begin{matrix} \frac{f x_{l p} m_{z}}{{x_{l p}}^{2} + {y_{l p}}^{2}} & \frac{f y_{l p} m_{z}}{{x_{l p}}^{2} + {y_{l p}}^{2}} & m_{2} \end{matrix}]}^{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{b}}_{h} = - α b_{h} + n_{ω 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:

X_{k} = [\begin{matrix} C_{n k}^{b} & b_{g k} & v_{k}^{n} & m_{v k}^{n} & m_{h k}^{n} & b_{h k} \end{matrix}]

(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:

$ξ_{i}^{n} = - {({[l_{v}^{n} \times]}^{2} + {[l_{h}^{n} \times]}^{2})}^{- 1} ([l_{v}^{n} \times] m_{v}^{n} + [l_{h}^{n} \times] m_{h}^{n})$

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

$\begin{matrix} ‖ ξ_{⊥ v}^{n} ‖ = ‖ ξ_{i}^{n} + ρ_{v} l_{v}^{n} ‖ = ‖ m_{v}^{n} ‖ \\ ‖ ξ_{⊥ h}^{n} ‖ = ‖ ξ_{i}^{n} + ρ_{h} l_{h}^{n} ‖ = ‖ m_{h}^{n} ‖ \end{matrix}$

(14)

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

$\begin{matrix} ρ_{v} = - ξ_{i}^{n T} l_{v}^{n} & ρ_{h} = - ξ_{i}^{n T} l_{h}^{n} \end{matrix}$

(15)
Calculate the relative position:

$\begin{matrix} ξ_{⊥ v}^{n} = ξ_{i}^{n} + ρ_{v} l_{v}^{n} = ξ_{i}^{n} - (ξ_{i}^{n T} l_{v}^{n}) l_{v}^{n} \\ ξ_{⊥ h}^{n} = ξ_{i}^{n} + ρ_{h} l_{h}^{n} = ξ_{i}^{n} - (ξ_{i}^{n T} l_{h}^{n}) l_{h}^{n} \end{matrix}$

(16)

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

C_{n k + 1}^{b} = Φ_{k} C_{n k}^{b} + [b_{g} \times] C_{n k}^{b} Δ t + w_{k}

(17)

where Φ_k is the transition matrix from time tk to time tk₊₁ that corresponds to

- [ω_{i b}^{b} \times]

. We assume that

ω_{i b}^{b}

is piece wisely constant in the tiny time intervals Δt = tk₊₁ − tk, and then, Φ_k in Equation (17) can be approximated as:

Φ_{k} \approx {exp}^{- [ω_{i b}^{b} \times] Δ t}

(18)

Furthermore, w_k can be written as:

w_{k} [n_{g} \times] C_{n k}^{b} Δ 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:

b_{g k + 1} = b_{g k} + n_{ω g} Δ t

(20)

v_{k + 1}^{n} = v_{k}^{n} + C_{b}^{n} f^{b} Δ t + g^{n} Δ t

(21)

m_{k + 1}^{n} = m_{k}^{n} + [l^{n} \times] v^{n} Δ t

(22)

b_{h k + 1} = {exp}^{- α Δ t} b_{h k} + n_{ω h} Δ t

(23)

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

X_{k + 1} = \sum_{r = 1}^{8} Θ_{k}^{r} X_{k} Λ_{k}^{r} + B_{k} U_{k} E + W_{k}

(24)

where the dynamic matrices,

Θ_{k}^{r}

Λ_{k}^{r}

, are defined as:

\begin{array}{l} Θ_{k}^{1} = Φ_{k} & Λ_{k}^{1} = E^{11} + E^{22} + E^{23} \\ Θ_{k}^{2} = [c_{1 k} \times] & Λ_{k}^{2} = E^{41} Δ t \\ Θ_{k}^{3} = [c_{2 k} \times] & Λ_{k}^{3} = E^{42} Δ t \\ Θ_{k}^{4} = [c_{3 k} \times] & Λ_{k}^{4} = E^{43} Δ t \\ Θ_{k}^{5} = I_{3} & Λ_{k}^{5} = E^{44} + E^{55} + E^{66} + E^{77} \\ Θ_{k}^{6} = [I_{v}^{n} \times] & Λ_{k}^{6} = E^{56} Δ t \\ Θ_{k}^{7} = [I_{h}^{n} \times] & Λ_{k}^{7} = E^{57} Δ t \\ Θ_{k}^{8} = {exp}^{- α Δ t} I_{3} & Λ_{k}^{8} = E^{88} \end{array}

(25)

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

B = [\begin{matrix} C_{n k}^{b T} & I_{3} \end{matrix}], U_{k} = [\begin{matrix} f_{k}^{b} \\ g^{n} \end{matrix}], E = [\begin{matrix} e_{5}^{T} Δ t & 0_{3 \times 1} \end{matrix}]

(26)

and the noise matrix, W_k, is defined as:

W_{k} = [\begin{array}{l} w_{k} & n_{ω g} Δ t & n_{a} Δ t & n_{c} Δ t & n_{c} Δ t & n_{h} Δ t \end{array}]

(27)

The process noise covariance matrix is:

Q_{k} = [\begin{matrix} cov [w_{k}] & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{3 \times 9} & σ_{ω g}^{2} Δ t I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & σ_{a}^{2} Δ t I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & σ_{c}^{2} Δ t I_{3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & σ_{c}^{2} Δ t I_{3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & σ_{h}^{2} \frac{1 - {exp}^{- 2 α Δ t}}{2 α} I_{3} \end{matrix}]

(28)

where, as defined by [11]:

cov [w_{k}] = ({({\hat{C}}_{n k}^{b})}^{T} \otimes I_{3}) L σ_{g}^{2} I_{3} L^{T} ({\hat{C}}_{n k}^{b} \otimes I_{3}) Δ t^{2}

(29)

L is the 9 × 3 matrix defined as:

L^{T} = [\begin{matrix} [e_{1} \times] & [e_{2} \times] & [e_{3} \times] \end{matrix}]

(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 $Θ_{k}^{r}$ , r = 2, 3, 4, are the functions of elements of $C_{n k}^{b}$ . One way of overcoming this difficulty is to substitute ${\hat{C}}_{n k / k}^{b}$ for $C_{n k}^{b}$ in the process equation. This substitution yields a pseudo-linear process equation.

3.2. Measurement Model

The matrix measurement equation:

Y_{k + 1} = [\begin{matrix} m_{k + 1}^{c} & m_{m k + 1} \end{matrix}] = \sum_{s = 1}^{2} H_{k + 1}^{s} X_{k + 1} G_{k + 1}^{s} + V_{k + 1}

(31)

where the measurement matrices are:

\begin{matrix} H_{k + 1}^{1} = C_{b}^{c}, G_{k + 1}^{1} = [\begin{matrix} m_{k}^{n} & 0_{3 \times 1} \\ 0_{5 \times 1} & 0_{5 \times 1} \end{matrix}] \\ H_{k + 1}^{2} = I_{3}, G_{k + 1}^{2} = [\begin{matrix} 0_{3 \times 1} & h + b_{h} \\ 0_{5 \times 1} & 0_{5 \times 1} \end{matrix}] \end{matrix}

(32)

and the measurement noise covariance matrix is given as:

R_{k + 1} = [\begin{matrix} σ_{c}^{2} I_{3} & 0_{3 \times 3} \\ 0_{3 \times 3} & σ_{h}^{2} I_{3} \end{matrix}]

(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]:

{\hat{X}}_{k + 1 / k} = \sum_{r = 1}^{8} Θ_{k}^{r} {\hat{X}}_{k / k} Λ_{k}^{r} + B_{k} U_{k} E

(34)

Φ_{k} = \sum_{r = 1}^{8} [{(Λ_{k}^{r})}^{T} \otimes Θ_{k}^{r}]

(35)

P_{k + 1 / k} = Φ_{k} P_{k / k} Φ_{k}^{T} + Q_{k}

(36)

Measurement update equations:

{\hat{Y}}_{k + 1} = \sum_{s = 1}^{2} H_{k + 1}^{s} {\hat{X}}_{k + 1 / k} G_{k + 1}^{s} - Y_{k + 1}

(37)

H_{k + 1} = \sum_{s = 1}^{2} [{(G_{k + 1}^{s})}^{T} \otimes H_{k + 1}^{s}]

(38)

S_{k + 1} = H_{k + 1} P_{k + 1 / k} H_{k + 1}^{T} + R_{k + 1}

(39)

K_{k + 1} = P_{k + 1} H_{k + 1}^{T} S_{k + 1}^{- 1}

(40)

{\hat{X}}_{k + 1 / k + 1} = {\hat{X}}_{k + 1 / k} - \sum_{j = 1}^{8} \sum_{l = 1}^{2} K_{k + 1}^{j l} {\tilde{Y}}_{k + 1} E^{l j}

(41)

where ⊗ denotes the Kronecker product [12],

K_{k + 1}^{j l}

is a 3 × 3 submatrix of the 24 × 6 matrix K_k+1 defined by:

K_{k + 1} = [\begin{matrix} K_{k + 1}^{11} & K_{k + 1}^{12} \\ ⋮ & ⋮ \\ K_{k + 1}^{81} & K_{k + 1}^{82} \end{matrix}]

(42)

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

P_{k + 1 / k + 1} = (I_{24} - K H) P_{k + 1 / k} {(I_{24} - K H)}^{T} + K R K^{T}

(43)

3.4. Orthogonalization

The Kalman filter is not designed to preserve any relationship among the components of the estimated state matrix ${\hat{C}}_{n k / 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]:

C_{n}^{b *} = {\hat{C}}_{n}^{b} (1.5 I_{3} - 0.5 {\hat{C}}_{n}^{b T} {\hat{C}}_{n}^{b})

(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):

C_{n}^{b *} = {({\hat{C}}_{n}^{b} {\hat{C}}_{n}^{b T})}^{- \frac{1}{2}} {\hat{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:

x (t) = {[\begin{matrix} c^{T} & b_{g}^{T} & v^{n T} & m_{v}^{n T} & m_{h}^{n T} & b_{n}^{T} \end{matrix}]}^{T}

(46)

where c is defined as:

\begin{matrix} c = [\begin{matrix} c_{1} \\ c_{2} \\ c_{3} \end{matrix}], & C_{n}^{b} = [\begin{matrix} c_{1} & c_{2} & c_{3} \end{matrix}] \end{matrix}

(47)

Equation (3) can be rewritten as:

\dot{c} = Ξ (b_{g}) c - Ξ (ω_{i b}^{b}) c = Ξ (b_{g}) c + T ω_{i b}^{b}

(48)

For arbitrary 3 × 1 vector p, we define:

Ξ (p) = [\begin{matrix} [p \times] & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & [p \times] & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & [p \times] \end{matrix}], T = [\begin{matrix} [c_{1} \times] \\ [c_{2} \times] \\ [c_{3} \times] \end{matrix}]

(49)

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

[\begin{matrix} \dot{c} \\ {\dot{b}}_{g} \\ {\dot{v}}_{g} \\ {\dot{m}}_{v}^{n} \\ {\dot{m}}_{h}^{n} \\ {\dot{b}}_{h} \end{matrix}] = \underset{f_{0}}{\underset{︸}{[\begin{matrix} Ξ (b_{g}) c \\ 0_{3 \times 1} \\ g^{n} \\ [I_{v}^{n} \times] v^{n} \\ [I_{h}^{n} \times] v^{n} \\ - α b_{h} \end{matrix}]}} + \underset{\underline{f_{1}}}{\underset{︸}{[\begin{matrix} T \\ 0_{3 \times 3} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \end{matrix}]}} ω_{i b}^{b} + \underset{\underline{f_{2}}}{\underset{︸}{[\begin{matrix} 0_{3 \times 3} \\ 0_{3 \times 3} \\ C_{b}^{n} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \end{matrix}]}} f^{b}

(50)

where

ω_{i b}^{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.,

\underline{f_{1}} ω_{i b}^{b} = f_{11} ω_{x} + f_{12} ω_{y} + f_{13} ω_{z}

(51)

where, for i = 1,2,3, f_l_i 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:

h^{m} = h_{1}^{*} (x) = C_{n}^{b} (h + b_{h}) = Ψ (h + b_{h}) c

(52)

m_{n}^{c} = h_{2}^{*} (x) = C_{b}^{c} C_{n}^{b} m_{v}^{n} = C_{b}^{c} Ψ (m_{v}^{n}) c

(53)

m_{h}^{c} = h_{3}^{*} (x) = C_{b}^{c} C_{n}^{b} m_{h}^{n} = C_{b}^{c} Ψ (m_{h}^{n}) c

(54)

where:

\begin{matrix} C_{n}^{b} p = Ψ (p) c, & p = {[\begin{matrix} p_{1} & p_{2} & p_{3} \end{matrix}]}^{T} \end{matrix}

(55)

Ψ (p) = [\begin{matrix} p_{1} I_{3} & p_{2} I_{3} & p_{3} I_{3} \end{matrix}]

(56)

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

h_{4}^{*} (x) = [\begin{matrix} {c_{1}}^{T} c_{1} \\ {c_{2}}^{T} c_{2} \\ {c_{3}}^{T} 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{matrix}] - [\begin{matrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{matrix}] = 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 $h_{1}^{*}$ , $h_{2}^{*}$ , $h_{3}^{*}$ , and $h_{4}^{*}$ , the ones whose gradients ensure that the observability matrix O is full rank.

The zeroth-Order Lie Derivatives $(𝔏^{0} h_{1}^{*}, 𝔏^{0} h_{2}^{*}, 𝔏^{0} h_{3}^{*}, 𝔏^{0} h_{4}^{*})$ : By definition, the zeroth-order Lie derivative of a function is the function itself, i.e.,

$𝔏^{0} h_{1}^{*} = h_{1}^{*} = C_{n}^{b} (h + b_{h}) = Ψ (h + b_{h}) c$

(58)

$𝔏^{0} h_{2}^{*} = h_{2}^{*} = C_{b}^{c} C_{n}^{b} m_{v}^{n} = C_{b}^{c} Ψ (m_{v}^{n}) c$

(59)

$𝔏^{0} h_{3}^{*} = h_{3}^{*} = C_{b}^{c} C_{n}^{b} m_{h}^{n} = C_{b}^{c} Ψ (m_{h}^{n}) c$

(60)

$𝔏^{0} h_{4}^{*} = h_{4}^{*} = [\begin{matrix} {c_{1}}^{T} c_{1} \\ {c_{2}}^{T} c_{2} \\ {c_{3}}^{T} 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{matrix}] - [\begin{matrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{matrix}]$

(61)

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

$\nabla 𝔏^{0} h_{1}^{*} = [\begin{matrix} Ψ (h + b_{h}) & 0_{3 \times 12} & C_{n}^{b} \end{matrix}]$

(62)

$\nabla 𝔏^{0} h_{2}^{*} = [\begin{matrix} C_{b}^{c} Ψ (m_{v}^{n}) & 0_{3 \times 6} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 6} \end{matrix}]$

(63)

$\nabla 𝔏^{0} h_{3}^{*} = [\begin{matrix} C_{b}^{c} Ψ (m_{h}^{n}) & 0_{3 \times 9} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} \end{matrix}]$

(64)

$\nabla 𝔏^{0} h_{4}^{*} = [\begin{matrix} 2 M & 0_{6 \times 15} \end{matrix}]$

(65)

where:

$M = [\begin{matrix} 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{matrix}]$

(66)
The first-Order Lie Derivatives $(\nabla 𝔏_{f_{0}}^{1} h_{2}^{*}, \nabla 𝔏_{f_{0}}^{1} h_{3}^{*}, \nabla 𝔏_{\underline{f_{1}}}^{1} h_{1}^{*},)$ : The first-order Lie derivatives of $h_{2}^{*}$ and $h_{3}^{*}$ with respect to f₀ are computed as:

$\begin{matrix} \nabla 𝔏_{f_{0}}^{1} h_{2}^{*} = \nabla 𝔏^{0} h_{2}^{*} \cdot f_{0} = C_{b}^{c} Ψ (m_{v}^{n}) Ξ (b_{g}) c + C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] v^{n} \\ = C_{b}^{c} [b_{g} \times] C_{n}^{b} m_{v}^{n} + C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] v^{n} \end{matrix}$

(67)

$\nabla 𝔏_{f_{0}}^{1} h_{3}^{*} = \nabla 𝔏^{0} h_{3}^{*} \cdot f_{0} = C_{b}^{c} [b_{g} \times] C_{n}^{b} m_{h}^{n} + C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] v^{n}$

(68)

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

\nabla 𝔏_{f_{0}}^{1} h_{2}^{*} = [\begin{matrix} X_{1} & - C_{b}^{c} [C_{n}^{b} m_{v}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 6} \end{matrix}]

(69)

\nabla 𝔏_{f_{0}}^{1} h_{3}^{*} = [\begin{matrix} X_{1} & - C_{b}^{c} [C_{n}^{b} m_{h}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] & 0_{3 \times 3} & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} \end{matrix}]

(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 $h_{1}^{*}$ with respect to f̠₁, i.e., $\nabla 𝔏_{\underline{f_{1}}}^{1} h_{1}^{*}$ . 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 𝔏_{\underline{f_{1}}}^{1} h_{1}^{*} = \nabla 𝔏^{0} h_{1}^{*} \cdot \underline{f_{1}} = Ψ (h + b_{h}) T

(71)

Stacking the gradients of the three columns together gives:

\nabla 𝔏_{\underline{f_{1}}}^{1} h_{1}^{*} = [\begin{matrix} Γ & 0_{9 \times 12} & γ \end{matrix}]

(72)

where the matrices:

Γ = [\begin{matrix} Γ_{1} \\ Γ_{2} \\ Γ_{3} \end{matrix}], γ = [\begin{matrix} γ_{1} \\ γ_{2} \\ γ_{3} \end{matrix}]

(73)

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

Γ_{i} = \frac{\partial}{\partial c} [Ψ (h + b_{h}) T e_{i}], γ_{i} = \frac{\partial}{\partial b_{h}} [Ψ (h + b_{h}) T e_{i}]

(74)

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

𝒪 = [\begin{matrix} \nabla 𝔏^{0} h_{1}^{*} \\ \nabla 𝔏^{0} h_{2}^{*} \\ \nabla 𝔏^{0} h_{3}^{*} \\ \nabla 𝔏_{f_{0}}^{1} h_{2}^{*} \\ \nabla 𝔏_{f_{0}}^{1} h_{3}^{*} \\ \nabla 𝔏_{\underline{f_{1}}}^{1} h_{1}^{*} \\ \nabla 𝔏^{0} h_{4}^{*} \end{matrix}] = [\begin{matrix} Ψ (h + b_{h}) & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{n}^{b} \\ C_{b}^{c} Ψ (m_{v}^{n}) & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ C_{b}^{c} Ψ (m_{h}^{n}) & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} \\ X_{1} & - C_{b}^{c} [C_{n}^{b} m_{v}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ X_{2} & - C_{b}^{c} [C_{n}^{b} m_{h}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] & 0_{3 \times 3} & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} \\ Γ & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & γ \\ 2 M & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} \end{matrix}]

(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:

$A = [\begin{matrix} Γ & γ \\ 2 M & 0_{6 \times 3} \end{matrix}]$

(76)

is rank 9 if at least one degree of rotational freedom is excited.
Lemma 2: The 6 × 6 matrix:

$B = [\begin{matrix} - C_{b}^{c} [C_{n}^{b} m_{v}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] \\ - C_{b}^{c} [C_{n}^{b} m_{h}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] \end{matrix}]$

(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. 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.

Acknowledgments

This work was supported in part by Research Fund for the Doctoral Program of Higher Education of China (20069998009), and the New Century Excellent Talents in University of China (NCET-07-0225).

Appendix A

For arbitrary two vector p and q

\begin{array}{l} Ψ (p) Ξ (q) c \\ = [\begin{matrix} p_{1} I_{3} & p_{2} I_{3} & p_{2} I_{3} \end{matrix}] [\begin{matrix} [q \times] & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & [q \times] & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & [q \times] \end{matrix}] [\begin{matrix} c_{1} \\ c_{2} \\ c_{3} \end{matrix}] \\ = p_{1} I_{3} [q \times] c_{1} + p_{2} I_{3} [q \times] c_{2} + p_{3} I_{3} [q \times] c_{3} \\ = [q \times] c_{1} p_{1} + [q \times] c_{2} p_{2} + [q \times] c_{3} p_{3} \\ = [\begin{matrix} [q \times] c_{1} & [q \times] c_{1} & [q \times] c_{1} \end{matrix}] [\begin{matrix} p_{1} \\ p_{2} \\ p_{3} \end{matrix}] \\ = [q \times] C_{n}^{b} p \end{array}

(A1)

Appendix B

We compute Γ and γ:

Γ = [\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & h_{1} & 0 & 0 & h_{2} & 0 & 0 & h_{3} \\ 0 & - h_{1} & 0 & 0 & - h_{2} & 0 & 0 & - h_{3} & 0 \\ 0 & 0 & - h_{1} & 0 & 0 & - h_{2} & 0 & 0 & - h_{3} \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ h_{1} & 0 & 0 & h_{2} & 0 & 0 & h_{3} & 0 & 0 \\ 0 & h_{1} & 0 & 0 & h_{2} & 0 & 0 & h_{3} & 0 \\ - h_{1} & 0 & 0 & - h_{2} & 0 & 0 & - h_{3} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{matrix}] \begin{matrix} \begin{matrix} 1 \\ 2 \\ 3 \end{matrix}} ω_{x} \\ \begin{matrix} 4 \\ 5 \\ 6 \end{matrix}} ω_{y} \\ \begin{matrix} 7 \\ 8 \\ 9 \end{matrix}} ω_{z} \end{matrix}

(B1)

γ = [\begin{matrix} 0 & 0 & 0 \\ c_{31} & c_{32} & c_{33} \\ - c_{21} & - c_{22} & - c_{23} \\ - c_{31} & - c_{32} & - c_{33} \\ 0 & 0 & 0 \\ c_{11} & c_{12} & c_{13} \\ c_{21} & c_{22} & c_{23} \\ - c_{11} & - c_{12} & - c_{13} \\ 0 & 0 & 0 \end{matrix}] \begin{matrix} \begin{matrix} 1 \\ 2 \\ 3 \end{matrix}} ω_{x} \\ \begin{matrix} 4 \\ 5 \\ 6 \end{matrix}} ω_{y} \\ \begin{matrix} 7 \\ 8 \\ 9 \end{matrix}} ω_{z} \end{matrix}

(B2)

where h + b_h = [h₁ h₂ h₃]^T. The variables, shown on the right side and next to the row numbers indicate the control input that is involved in using the corresponding row numbers: when a row is retained in the observability matrix (e.g., the second row), the underlying assumption is that the corresponding control input (i.e., ω_x) is nonzero.

If none of the components in h + b_h is zero, we can obtain:

det (A {4, 6, 7, 10 : 15}) \neq 0

(B3)

det (A {2, 3, 8, 10 : 15}) \neq 0

(B4)

det (A {2, 3, 6, 10 : 15}) \neq 0

(B5)

It is worth-mentioning that each selection of the rows is based on two nonzero components of $ω_{i b}^{b} = {[ω_{x} ω_{y} ω_{z}]}^{T}$ . For example, we can use either Equation (B3) which assumes that ω_y and ω_z are nonzero, or Equation (B4) which assumes that ω_x and ω_z are nonzero, or Equation (B5) which assumes that ω_x and ω_y are nonzero.

Only if the rotational axis does not coincide with b coordinate axis, will one degree of rotation have two nonzero components of $ω_{i b}^{b}$ . The rank of A is 9 if at last one degree of rotational freedom is excited.

Appendix C

| B | = | C_{n}^{b} | | [\begin{matrix} - [C_{n}^{b} m_{v}^{b} \times] & C_{n}^{b} [l_{v}^{b} \times] \\ - [C_{n}^{b} m_{h}^{b} \times] & C_{n}^{b} [l_{h}^{b} \times] \end{matrix}] |

(C1)

Note that $C_{b}^{c}$ is full rank.

det (B) = (l_{h 2} m_{h 1} - l_{h 1} m_{h 2}) (m_{v 1} m_{h 2} - m_{v 2} m_{h 1}) + {(l_{h 2} m_{v 1} - l_{h 1} m_{v 2})}^{2} m_{h 3}

(C2)

where:

l_{v}^{n} = {[\begin{matrix} 0 & 0 & 1 \end{matrix}]}^{T}, m_{v}^{n} = {[\begin{matrix} m_{v 1} & m_{v 2} & 0 \end{matrix}]}^{T}

(C3)

l_{h}^{n} = {[\begin{matrix} l_{h 1} & l_{h_{2}} & 0 \end{matrix}]}^{T}, m_{h}^{n} = {[\begin{matrix} m_{h 1} & m_{h 2} & m_{h 3} \end{matrix}]}^{T}

(C4)

det(B) = 0 when: (a); m_h₁ = m_h₂= 0 (b)

l_{h}^{n}

and

m_{v}^{n}

are linearly dependent. Only (a) and (b) are both satisfied, B is not full rank. To avoid the situation, two linearly independent horizontal lines are observed or the line in the plane (zⁿ = 0) is not chosen. In practice, the former is more feasible. Even one of horizontal line is linearly dependent with

m_{v}^{n}

, the another is linearly independent with

m_{v}^{n}

. Then, (b) is not satisfied. So B is full rank if two linearly independent horizontal lines are observed.

Appendix D

Step (1)
Based on Lemma 1, the rank of A is 9. We diagonalize it with the Gaussian elimination:

$[\begin{matrix} Ψ (h + b_{h}) & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{n}^{b} \\ C_{b}^{c} Ψ (m_{v}^{n}) & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ C_{b}^{c} Ψ (m_{h}^{n}) & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} \\ X_{1} & - C_{b}^{c} [C_{n}^{b} m_{v}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ X_{2} & - C_{b}^{c} [C_{n}^{b} m_{h}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] & 0_{3 \times 3} & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} \\ I_{9} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{6 \times 9} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} \end{matrix}]$

(D1)

Then we can easily eliminate all other elements of the first columns:

$[\begin{matrix} 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{n}^{b} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} \\ 0_{3 \times 9} & - C_{b}^{c} [C_{n}^{b} m_{v}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & - C_{b}^{c} [C_{n}^{b} m_{h}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] & 0_{3 \times 3} & C_{b}^{c} [b_{g} \times] C_{n}^{b} & 0_{3 \times 3} \\ I_{9} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{6 \times 9} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} \end{matrix}]$

(D2)
Step (2)
We can eliminate all other elements of the fourth and fifth columns using $C_{b}^{c} C_{n}^{b}$ , respectively:

$[\begin{matrix} 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{n}^{b} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} \\ 0_{3 \times 9} & - C_{b}^{c} [C_{n}^{b} m_{v}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{v}^{n} \times] & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & - C_{b}^{c} [C_{n}^{b} m_{h}^{n} \times] & C_{b}^{c} C_{n}^{b} [l_{h}^{n} \times] & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ I_{9} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{6 \times 9} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} \end{matrix}]$

(D3)
Step (3)
Based on Lemma 2, B is full rank, thus we diagonalize it with the Gaussian elimination:

$[\begin{matrix} 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{n}^{b} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{c} C_{n}^{b} & 0_{3 \times 3} \\ 0_{3 \times 9} & I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 9} & 0_{3 \times 3} & I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ I_{9} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} & 0_{6 \times 3} \end{matrix}]$

(D4)

Note that $C_{b}^{c} C_{n}^{b}$ and $C_{n}^{b}$ are both full rank. It is easy to see the matrix is full rank, indicating that the matrix O is also full rank.

References

Titterton, D.H.; Weston, J.L. Strapdown Inertial Navigation Technology, 2nd ed.; Peter Peregrinus Ltd. on behalf of the Institute of Electrical Engineers: London, UK, 2004. [Google Scholar]
Dias, J.; Vinzce, M.; Corke, P.; Lobo, J. Special issue: 2nd workshop on integration of vision and inertial sensors. Int. J. Robot. Res. 2007, 26, 519–535. [Google Scholar]
Sabatini, A.M. Estimating three-dimensional orientation of human body parts by inertial/magnetic sensing. Sensors 2011, 11, 1489–1525. [Google Scholar]
Wahha, G. A least-square estimate of spacecraft attitude. SLAM Rev. 1965, 7, 409. [Google Scholar]
Choukroun, D.; Weiss, H.; Bar-Itzhack, I.Y.; Oshman, Y. Kalman filtering for matrix estimation. IEEE Trans. Aerosp. Elect. Syst. 2006, 42, 147–159. [Google Scholar]
Sabatini, A.M. Kalman-filter-based orientation determination using inertial/magnetic sensors: Observability analysis and performance evaluation. Sensors 2011, 11, 9182–9206. [Google Scholar]
Groves, P.D. Principles of GNSS, Inertial, and Multi-Sensor Integrated Navigation Systems; Artech House: Norwood, MA, USA, 2008. [Google Scholar]
Ferraris, F.; Grimaldi, U.; Parvis, M. Produre for effortless in-field calibration of three-axis rate gyros and accelerometers. Sens. Mater. 1995, 7, 311–330. [Google Scholar]
Goddard, J.S. Pose and motion estimation from vision using dual quaternion-based extended Kalman filtering. Proc. SPIE 1997, 3313, 189–200. [Google Scholar]
Roetenberg, D.; Luinge, H.J.; Baten, C.T.M.; Veltink, P.H. Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation. IEEE Trans. Neur. Syst. Rehabil. Eng. 2005, 12, 395–405. [Google Scholar]
Choukroun, D.; Weiss, H.; Bar-Itzhack, I.Y.; Oshman, Y. Direction Cosine Matrix Estimation from Vector Observations Using a Matrix Kalman Filter. Proceedings of AIAA Guidance, Navigation, and Control Conference and Exhibit, Austin, TX, USA, 11– 14 August 2003.
Bellman, R. Introduction to Matrix Analysis; McGraw-Hill: New York, NY, USA, 1960. [Google Scholar]
Bar-Itzhack, I.Y.; Reiner, J. Recursive attitude determination from vector observations: DCM identification. J. Guid. Contr. Dynam. 1984, 7, 51–56. [Google Scholar]
Bar-Itzhack, I.Y.; Meyer, J. On the convergence of iterative orthogonalization processes. IEEE Trans. Aerosp. Electron. Syst. 1976, 12, 146–151. [Google Scholar]
Brogan, W.L. Modern Control Theory; Prentice-Hall: Upper Saddle River, NJ, USA, 1990. [Google Scholar]
Hermann, R.; Krener, A. Nonlinear controlability and observability. IEEE Trans. Autom. Control 1977, 22, 728–740. [Google Scholar]
Sastry, S. Nonlinear Systems: Analysis, Stability, and Control; Springer-Verlag: New York, NY, USA, 1999. [Google Scholar]
Oliensis, J. A new structure-from-motion ambiguity. IEEE Trans. Pattern Anal. Mach. Intell. 2000, 22, 685–700. [Google Scholar]

Figure 1. Geometry of visual/inertial/magnetic sensor based navigation.

Figure 2. The estimated 3-D trajectory of the pushcart.

Figure 3. The 3σ bounds for the errors in the attitude matrix and the velocities in the n frame.

Table 1. Accuracies of the Sensors.

**Table 1.** Accuracies of the Sensors.
Sensors	Number	Accuracies	Sampling Rates
IMU	1	gyro drift: <0.01°/s (short term)	100 Hz
IMU	1	accelerometer bias: ±2 mg (short term)	100 Hz
Camera	1	Resolution: 656 × 490	10 Hz
		focus length: 25 mm
		field of view: 60°
Magnetometer	1	σ_bh:10 u.a./s(×10⁻³)	100 Hz
		α:10 s⁻¹
		σ_h:1 mGauss

© 2012 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

Share and Cite

MDPI and ACS Style

Feng, G.; Wu, W.; Wang, J. Observability Analysis of a Matrix Kalman Filter-Based Navigation System Using Visual/Inertial/Magnetic Sensors. Sensors 2012, 12, 8877-8894. https://doi.org/10.3390/s120708877

AMA Style

Feng G, Wu W, Wang J. Observability Analysis of a Matrix Kalman Filter-Based Navigation System Using Visual/Inertial/Magnetic Sensors. Sensors. 2012; 12(7):8877-8894. https://doi.org/10.3390/s120708877

Chicago/Turabian Style

Feng, Guohu, Wenqi Wu, and Jinling Wang. 2012. "Observability Analysis of a Matrix Kalman Filter-Based Navigation System Using Visual/Inertial/Magnetic Sensors" Sensors 12, no. 7: 8877-8894. https://doi.org/10.3390/s120708877

APA Style

Feng, G., Wu, W., & Wang, J. (2012). Observability Analysis of a Matrix Kalman Filter-Based Navigation System Using Visual/Inertial/Magnetic Sensors. Sensors, 12(7), 8877-8894. https://doi.org/10.3390/s120708877

Article Menu