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

Article
Observability Analysis of a Matrix Kalman Filter-Based Navigation System Using Visual/Inertial/Magnetic Sensors
Guohu Feng 1,*, Wenqi Wu 1 and Jinling Wang 2
1
The College of Mechatronics and Automation, National University of Defense Technology, Changsha 410073, Hunan, China; E-Mail: wenqiwu_lit@hotmail.com
2
School of Surveying and Spatial Information Systems, University of New South Wales, Sydney, NSW 2052, Australia; E-Mail: jinling.wang@unsw.edu.au
*
Author to whom correspondence should be addressed; guohu_feng@hotmail.com; Tel./Fax: +86-731-8457-6463.
Received: 14 May 2012; in revised form: 14 June 2012 / Accepted: 18 June 2012 /
Published: 27 June 2012

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 vn and the body attitude matrix C n b satisfy the kinematic equations as [1,7]:

ξ ˙ n = - v n
v ˙ n = C b n ( f b - b a ) + g n
C ˙ n b = - [ ω n b b × ] C n b , ω n b b = ω i b b - b g
b ˙ g = n ω g
where ξn is an arbitrary point on the observed line, rb is the lever arm from the IMU to the camera, as shown in Figure 1. ba and bg are 3 × 1 vectors that describe the biases affecting the accelerometer and gyro measurements, respectively. ba can be compensated on time scales up to few hours, using the procedure described in [8]. bg are modeled as random walk processes, driven by the white Gaussian noise vectors nωg. [ ω n b b × ] 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 = f m x c m z c m x c 2 + m y c 2 , y l p = f m y c m z c m x c 2 + m y c 2

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:

m n = ξ n × l n
while in the c-frame:
m c = ( C n c ( ξ n - C n c r b ) ) × ( C n c l n ) = C n c ( ( ξ n - C n c r b ) × l n ) = C n c m n - C n c r b × l n C n c m n
where rb can be ignored when the IMU and the camera are mounted closely together.

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

m n = ξ n l n = ξ n

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

m ˙ n = ξ n × l n = - v n × l n = [ l n × ] v n

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 = [ m x m y m z ] T = [ f x l p m z x l p 2 + y l p 2 f y l p m z x l p 2 + y l p 2 m 2 ] T

A monocular camera is not enough to calculate mz 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 mz.

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

b ˙ h = - α b h + n ω h
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 = [ C n k b b g k v k n m v k n m h k n b h k ]
where mn is the relative distance of the vehicle with respect to the observed line, vn is the relative velocity.

The relative position of the vehicle with respect to the observed line, ξn, 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 × ] 2 + [ l h n × ] 2 ) - 1 ( [ l v n × ] m v n + [ l h n × ] m h n )

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

    ξ v n = ξ i n + ρ v l v n = m v n ξ h n = ξ i n + ρ h l h n = m h n

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

    ρ v = - ξ i n T l v n ρ h = - ξ i n T l h n

  • Calculate the relative position:

    ξ 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

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

C n k + 1 b = Φ k C n k b + [ b g × ] C n k b Δ t + w k
where Φk is the transition matrix from time tk to time tk+1 that corresponds to - [ ω i b b × ]. We assume that ω i b b is piece wisely constant in the tiny time intervals Δt = tk+1tk, and then, Φk in Equation (17) can be approximated as:
Φ k exp - [ ω i b b × ] Δ t

Furthermore, wk can be written as:

w k [ n g × ] C n k b Δ t + h . o . t
where h.o.t denotes the terms of the second order Δt2 and higher. Note that the process noise matrix, wk, is state dependent, and that the first-order term is linear in the components of the white Gaussian noise vectors ng.

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

b g k + 1 = b g k + n ω g Δ t
v k + 1 n = v k n + C b n f b Δ t + g n Δ t
m k + 1 n = m k n + [ l n × ] v n Δ t
b h k + 1 = exp - α Δ t b h k + n ω h Δ t

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

X k + 1 = r = 1 8 Θ k r X k Λ k r + B k U k E + W k
where the dynamic matrices, Θ k r, Λ k r, are defined as:
Θ k 1 = Φ k Λ k 1 = E 11 + E 22 + E 23 Θ k 2 = [ c 1 k × ] Λ k 2 = E 41 Δ t Θ k 3 = [ c 2 k × ] Λ k 3 = E 42 Δ t Θ k 4 = [ c 3 k × ] Λ k 4 = E 43 Δ t Θ k 5 = I 3 Λ k 5 = E 44 + E 55 + E 66 + E 77 Θ k 6 = [ I v n × ] Λ k 6 = E 56 Δ t Θ k 7 = [ I h n × ] Λ k 7 = E 57 Δ t Θ k 8 = exp - α Δ t I 3 Λ k 8 = E 88
where Eij denotes a 8 × 8 matrix with 1 at position (ij) and 0 elsewhere;
B = [ C n k b T I 3 ] , U k = [ f k b g n ] , E = [ e 5 T Δ t 0 3 × 1 ]
and the noise matrix, Wk, is defined as:
W k = [ w k n ω g Δ t n a Δ t n c Δ t n c Δ t n h Δ t ]

The process noise covariance matrix is:

Q k = [ cov [ w k ] 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 3 × 9 σ ω g 2 Δ t I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 σ a 2 Δ t I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 0 3 × 3 σ c 2 Δ t I 3 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 σ c 2 Δ t I 3 0 3 × 3 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 σ h 2 1 - exp - 2 α Δ t 2 α I 3 ]
where, as defined by [11]:
cov [ w k ] = ( ( C ^ n k b ) T I 3 ) L σ g 2 I 3 L T ( C ^ n k b I 3 ) Δ t 2

L is the 9 × 3 matrix defined as:

L T = [ [ e 1 × ] [ e 2 × ] [ e 3 × ] ]
with e1 = [1 0 0]T, e2 = [0 1 0]T, and e3 = [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 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 = [ m k + 1 c m m k + 1 ] = s = 1 2 H k + 1 s X k + 1 G k + 1 s + V k + 1
where the measurement matrices are:
H k + 1 1 = C b c , G k + 1 1 = [ m k n 0 3 × 1 0 5 × 1 0 5 × 1 ] H k + 1 2 = I 3 , G k + 1 2 = [ 0 3 × 1 h + b h 0 5 × 1 0 5 × 1 ]
and the measurement noise covariance matrix is given as:
R k + 1 = [ σ c 2 I 3 0 3 × 3 0 3 × 3 σ h 2 I 3 ]

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

X ^ k + 1 / k = r = 1 8 Θ k r X ^ k / k Λ k r + B k U k E
Φ k = r = 1 8 [ ( Λ k r ) T Θ k r ]
P k + 1 / k = Φ k P k / k Φ k T + Q k

Measurement update equations:

Y ^ k + 1 = s = 1 2 H k + 1 s X ^ k + 1 / k G k + 1 s - Y k + 1
H k + 1 = s = 1 2 [ ( G k + 1 s ) T H k + 1 s ]
S k + 1 = H k + 1 P k + 1 / k H k + 1 T + R k + 1
K k + 1 = P k + 1 H k + 1 T S k + 1 - 1
X ^ k + 1 / k + 1 = X ^ k + 1 / k - j = 1 8 l = 1 2 K k + 1 j l Y ˜ k + 1 E l j
where ⊗ denotes the Kronecker product [12], K k + 1 j l is a 3 × 3 submatrix of the 24 × 6 matrix Kk+1 defined by:
K k + 1 = [ K k + 1 11 K k + 1 12 K k + 1 81 K k + 1 82 ]
and Elj 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

3.4. Orthogonalization

The Kalman filter is not designed to preserve any relationship among the components of the estimated state matrix 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 = C ^ n b ( 1.5 I 3 - 0.5 C ^ n b T C ^ n b )

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 = ( C ^ n b C ^ n b T ) - 1 2 C ^ n b

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 ) = [ c T b g T v n T m v n T m h n T b n T ] T
where c is defined as:
c = [ c 1 c 2 c 3 ] , C n b = [ c 1 c 2 c 3 ]

Equation (3) can be rewritten as:

c ˙ = Ξ ( b g ) c - Ξ ( ω i b b ) c = Ξ ( b g ) c + T ω i b b

For arbitrary 3 × 1 vector p, we define:

Ξ ( p ) = [ [ p × ] 0 3 × 3 0 3 × 3 0 3 × 3 [ p × ] 0 3 × 3 0 3 × 3 0 3 × 3 [ p × ] ] , T = [ [ c 1 × ] [ c 2 × ] [ c 3 × ] ]

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

[ c ˙ b ˙ g v ˙ g m ˙ v n m ˙ h n b ˙ h ] = [ Ξ ( b g ) c 0 3 × 1 g n [ I v n × ] v n [ I h n × ] v n - α b h ] f 0 + [ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] f 1 _ ω i b b + [ 0 3 × 3 0 3 × 3 C b n 0 3 × 3 0 3 × 3 0 3 × 3 ] f 2 _ f b
where ω i b b and fb are the gyro and accelerometer measurements.

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

f 1 _ ω i b b = f 11 ω x + f 12 ω y + f 13 ω z
where, for i = 1,2,3, fli denotes the i th column vector comprising f̠1 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
m n c = h 2 ( x ) = C b c C n b m v n = C b c Ψ ( m v n ) c
m h c = h 3 ( x ) = C b c C n b m h n = C b c Ψ ( m h n ) c
where:
C n b p = Ψ ( p ) c , p = [ p 1 p 2 p 3 ] T
Ψ ( p ) = [ p 1 I 3 p 2 I 3 p 3 I 3 ]

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

h 4 ( x ) = [ 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 ] - [ 1 1 1 1 1 1 ] = 0

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
    𝔏 0 h 2 = h 2 = C b c C n b m v n = C b c Ψ ( m v n ) c
    𝔏 0 h 3 = h 3 = C b c C n b m h n = C b c Ψ ( m h n ) c
    𝔏 0 h 4 = h 4 = [ 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 ] - [ 1 1 1 1 1 1 ]

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

    𝔏 0 h 1 = [ Ψ ( h + b h ) 0 3 × 12 C n b ]
    𝔏 0 h 2 = [ C b c Ψ ( m v n ) 0 3 × 6 C b c C n b 0 3 × 6 ]
    𝔏 0 h 3 = [ C b c Ψ ( m h n ) 0 3 × 9 C b c C n b 0 3 × 3 ]
    𝔏 0 h 4 = [ 2 M 0 6 × 15 ]
    where:
    M = [ 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 ]

  • The first-Order Lie Derivatives ( 𝔏 f 0 1 h 2 , 𝔏 f 0 1 h 3 , 𝔏 f 1 _ 1 h 1 , ): The first-order Lie derivatives of h 2 and h 3 with respect to f0 are computed as:

    𝔏 f 0 1 h 2 = 𝔏 0 h 2 f 0 = C b c Ψ ( m v n ) Ξ ( b g ) c + C b c C n b [ l v n × ] v n = C b c [ b g × ] C n b m v n + C b c C n b [ l v n × ] v n
    𝔏 f 0 1 h 3 = 𝔏 0 h 3 · f 0 = C b c [ b g × ] C n b m h n + C b c C n b [ l h n × ] v n

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

𝔏 f 0 1 h 2 = [ X 1 - C b c [ C n b m v n × ] C b c C n b [ l v n × ] C b c [ b g × ] C n b 0 3 × 6 ]
𝔏 f 0 1 h 3 = [ X 1 - C b c [ C n b m h n × ] C b c C n b [ l h n × ] 0 3 × 3 C b c [ b g × ] C n b 0 3 × 3 ]

In these expressions, X1 and X2 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̠1, i.e., 𝔏 f 1 _ 1 h 1 . At this point, it is important to note that f̠1 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):

𝔏 f 1 _ 1 h 1 = 𝔏 0 h 1 f 1 _ = Ψ ( h + b h ) T

Stacking the gradients of the three columns together gives:

𝔏 f 1 _ 1 h 1 = [ Γ 0 9 × 12 γ ]
where the matrices:
Γ = [ Γ 1 Γ 2 Γ 3 ] , γ = [ γ 1 γ 2 γ 3 ]
of dimensions 9 × 9 and 9 × 3, have the block-row elements (for i = 1,2,3) as follows:
Γ i = c [ Ψ ( h + b h ) T e i ] , γ i = b h [ Ψ ( h + b h ) T e i ]

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

𝒪 = [ 𝔏 0 h 1 𝔏 0 h 2 𝔏 0 h 3 𝔏 f 0 1 h 2 𝔏 f 0 1 h 3 𝔏 f 1 _ 1 h 1 𝔏 0 h 4 ] = [ Ψ ( h + b h ) 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 C n b C b c Ψ ( m v n ) 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 3 C b c Ψ ( m h n ) 0 3 × 3 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 X 1 - C b c [ C n b m v n × ] C b c C n b [ l v n × ] C b c [ b g × ] C n b 0 3 × 3 0 3 × 3 X 2 - C b c [ C n b m h n × ] C b c C n b [ l h n × ] 0 3 × 3 C b c [ b g × ] C n b 0 3 × 3 Γ 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 γ 2 M 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 ]

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 = [ Γ γ 2 M 0 6 × 3 ]
    is rank 9 if at least one degree of rotational freedom is excited.

  • Lemma 2: The 6 × 6 matrix:

    B = [ - C b c [ C n b m v n × ] C b c C n b [ l v n × ] - C b c [ C n b m h n × ] C b c C n b [ l h n × ] ]
    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. mc 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.

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

Ψ ( p ) Ξ ( q ) c = [ p 1 I 3 p 2 I 3 p 2 I 3 ] [ [ q × ] 0 3 × 3 0 3 × 3 0 3 × 3 [ q × ] 0 3 × 3 0 3 × 3 0 3 × 3 [ q × ] ] [ c 1 c 2 c 3 ] = p 1 I 3 [ q × ] c 1 + p 2 I 3 [ q × ] c 2 + p 3 I 3 [ q × ] c 3 = [ q × ] c 1 p 1 + [ q × ] c 2 p 2 + [ q × ] c 3 p 3 = [ [ q × ] c 1 [ q × ] c 1 [ q × ] c 1 ] [ p 1 p 2 p 3 ] = [ q × ] C n b p

Appendix B

We compute Γ and γ:

Γ = [ 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 ] 1 2 3 } ω x 4 5 6 } ω y 7 8 9 } ω z
γ = [ 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 ] 1 2 3 } ω x 4 5 6 } ω y 7 8 9 } ω z
where h + bh = [h1 h2 h3]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 + bh is zero, we can obtain:

det ( A { 4 , 6 , 7 , 10 : 15 } ) 0
det ( A { 2 , 3 , 8 , 10 : 15 } ) 0
det ( A { 2 , 3 , 6 , 10 : 15 } ) 0

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 | | [ - [ C n b m v b × ] C n b [ l v b × ] - [ C n b m h b × ] C n b [ l h b × ] ] |

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
where:
l v n = [ 0 0 1 ] T , m v n = [ m v 1 m v 2 0 ] T
l h n = [ l h 1 l h 2 0 ] T , m h n = [ m h 1 m h 2 m h 3 ] T
det(B) = 0 when: (a); mh1 = mh2= 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 (zn = 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:

    [ Ψ ( h + b h ) 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 C n b C b c Ψ ( m v n ) 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 3 C b c Ψ ( m h n ) 0 3 × 3 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 X 1 - C b c [ C n b m v n × ] C b c C n b [ l v n × ] C b c [ b g × ] C n b 0 3 × 3 0 3 × 3 X 2 - C b c [ C n b m h n × ] C b c C n b [ l h n × ] 0 3 × 3 C b c [ b g × ] C n b 0 3 × 3 I 9 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 6 × 9 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 ]

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

    [ 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 C n b 0 3 × 9 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 9 - C b c [ C n b m v n × ] C b c C n b [ l v n × ] C b c [ b g × ] C n b 0 3 × 3 0 3 × 3 0 3 × 9 - C b c [ C n b m h n × ] C b c C n b [ l h n × ] 0 3 × 3 C b c [ b g × ] C n b 0 3 × 3 I 9 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 6 × 9 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 ]

  • Step (2)

    We can eliminate all other elements of the fourth and fifth columns using C b c C n b, respectively:

    [ 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 C n b 0 3 × 9 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 9 - C b c [ C n b m v n × ] C b c C n b [ l v n × ] 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 9 - C b c [ C n b m h n × ] C b c C n b [ l h n × ] 0 3 × 3 0 3 × 3 0 3 × 3 I 9 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 6 × 9 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 ]

  • Step (3)

    Based on Lemma 2, B is full rank, thus we diagonalize it with the Gaussian elimination:

    [ 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 C n b 0 3 × 9 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 0 3 × 3 0 3 × 3 C b c C n b 0 3 × 3 0 3 × 9 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 9 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 I 9 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 9 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 0 6 × 3 ]

    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

  1. Titterton, D.H.; Weston, J.L. Strapdown Inertial Navigation Technology, 2nd ed. ed.; Peter Peregrinus Ltd. on behalf of the Institute of Electrical Engineers: London, UK, 2004.
  2. 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, doi:10.1177/0278364907079279.
  3. Sabatini, A.M. Estimating three-dimensional orientation of human body parts by inertial/magnetic sensing. Sensors 2011, 11, 1489–1525, doi:10.3390/s110201489.
  4. Wahha, G. A least-square estimate of spacecraft attitude. SLAM Rev. 1965, 7, 409.
  5. Choukroun, D.; Weiss, H.; Bar-Itzhack, I.Y.; Oshman, Y. Kalman filtering for matrix estimation. IEEE Trans. Aerosp. Elect. Syst. 2006, 42, 147–159, doi:10.1109/TAES.2006.1603411.
  6. Sabatini, A.M. Kalman-filter-based orientation determination using inertial/magnetic sensors: Observability analysis and performance evaluation. Sensors 2011, 11, 9182–9206, doi:10.3390/s111009182.
  7. Groves, P.D. Principles of GNSS, Inertial, and Multi-Sensor Integrated Navigation Systems; Artech House: Norwood, MA, USA, 2008.
  8. 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.
  9. Goddard, J.S. Pose and motion estimation from vision using dual quaternion-based extended Kalman filtering. Proc. SPIE 1997, 3313, 189–200.
  10. 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.
  11. 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.
  12. Bellman, R. Introduction to Matrix Analysis; McGraw-Hill: New York, NY, USA, 1960.
  13. Bar-Itzhack, I.Y.; Reiner, J. Recursive attitude determination from vector observations: DCM identification. J. Guid. Contr. Dynam. 1984, 7, 51–56, doi:10.2514/3.56362.
  14. Bar-Itzhack, I.Y.; Meyer, J. On the convergence of iterative orthogonalization processes. IEEE Trans. Aerosp. Electron. Syst. 1976, 12, 146–151, doi:10.1109/TAES.1976.308289.
  15. Brogan, W.L. Modern Control Theory; Prentice-Hall: Upper Saddle River, NJ, USA, 1990.
  16. Hermann, R.; Krener, A. Nonlinear controlability and observability. IEEE Trans. Autom. Control 1977, 22, 728–740, doi:10.1109/TAC.1977.1101601.
  17. Sastry, S. Nonlinear Systems: Analysis, Stability, and Control; Springer-Verlag: New York, NY, USA, 1999.
  18. Oliensis, J. A new structure-from-motion ambiguity. IEEE Trans. Pattern Anal. Mach. Intell. 2000, 22, 685–700, doi:10.1109/34.865186.
Sensors 12 08877f1 200
Figure 1. Geometry of visual/inertial/magnetic sensor based navigation.

Click here to enlarge figure

Figure 1. Geometry of visual/inertial/magnetic sensor based navigation.
Sensors 12 08877f1 1024
Sensors 12 08877f2 200
Figure 2. The estimated 3-D trajectory of the pushcart.

Click here to enlarge figure

Figure 2. The estimated 3-D trajectory of the pushcart.
Sensors 12 08877f2 1024
Sensors 12 08877f3a 200Sensors 12 08877f3b 200
Figure 3. The 3σ bounds for the errors in the attitude matrix and the velocities in the n frame.

Click here to enlarge figure

Figure 3. The 3σ bounds for the errors in the attitude matrix and the velocities in the n frame.
Sensors 12 08877f3a 1024Sensors 12 08877f3b 1024
Table Table 1. Accuracies of the Sensors.

Click here to display table

Table 1. Accuracies of the Sensors.
SensorsNumberAccuraciesSampling Rates
IMU1gyro drift: <0.01°/s (short term)100 Hz
accelerometer bias: ±2 mg (short term)
Camera1Resolution: 656 × 49010 Hz
focus length: 25 mm
field of view: 60°
Magnetometer1σbh:10 u.a./s(×10−3)100 Hz
α:10 s−1
σh:1 mGauss
Sensors EISSN 1424-8220 Published by MDPI AG, Basel, Switzerland RSS E-Mail Table of Contents Alert