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/).
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.
matrix Kalman filterLie derivativesobservability of nonlinear systemsnavigationvisioninertial measurement unitIntroduction
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.
Sensor ModelingInertial 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
Cnb satisfy the kinematic equations as [1,7]:
ξ˙n=-vnv˙n=Cbn(fb-ba)+gnC˙nb=-[ωnbb×]Cnb,ωnbb=ωibb-bgb˙g=nωgwhere ξ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.
[ωnbb×] is the skew symmetric matrix of
ωnbb. 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.
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]:
xlp=fmxcmzcmxc2+myc2,ylp=fmycmzcmxc2+myc2
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:
mn=ξn×lnwhile in the c-frame:
mc=(Cnc(ξn-Cncrb))×(Cncln)=Cnc((ξn-Cncrb)×ln)=Cncmn-Cncrb×ln≈Cncmnwhere 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:
‖mn‖=‖ξn‖‖ln‖=‖ξ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×ln=-vn×ln=[ln×]vn
For simplification, only vertical lines
lvn and horizontal lines
lhn are chosen as landmarks. According to Equation (5), we obtain:
mc=[mxmymz]T=[fxlpmzxlp2+ylp2fylpmzxlp2+ylp2m2]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.
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=-αbh+nωhwhere α is a positive constant, and nωh is white Gaussian noise.
MKF AlgorithmProcess Model
The evolving state is described as follows:
Xk=[Cnkbbgkvknmvknmhknbhk]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:
ξin=-([lvn×]2+[lhn×]2)-1([lvn×]mvn+[lhn×]mhn)
Calculate the translation along the observed line direction from the intersection to the vertical point. According to Equation (8), we obtain:
‖ξ⊥vn‖=‖ξin+ρvlvn‖=‖mvn‖‖ξ⊥hn‖=‖ξin+ρhlhn‖=‖mhn‖
Because the unique of perpendicular point, the translation along the observed line direction, ρ, is also uniqueness. From Equation (14), we obtain:
ρv=-ξinTlvnρh=-ξinTlhn
Calculate the relative position:
ξ⊥vn=ξin+ρvlvn=ξin-(ξinTlvn)lvnξ⊥hn=ξin+ρhlhn=ξin-(ξinTlhn)lhn
The discretization of Equation (3) gives the following equation [11]:
Cnk+1b=ΦkCnkb+[bg×]CnkbΔt+wkwhere Φk is the transition matrix from time tk to time tk+1 that corresponds to
-[ωibb×]. We assume that
ωibb is piece wisely constant in the tiny time intervals Δt = tk+1 − tk, and then, Φk in Equation (17) can be approximated as:
Φk≈exp-[ωibb×]Δt
Furthermore, wk can be written as:
wk[ng×]CnkbΔt+h.o.twhere 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.
Combining Equations (17), (20)–(23) together, we obtain the dynamic model as:
Xk+1=∑r=18ΘkrXkΛkr+BkUkE+Wkwhere the dynamic matrices,
Θkr,
Λkr, are defined as:
Θk1=ΦkΛk1=E11+E22+E23Θk2=[c1k×]Λk2=E41ΔtΘk3=[c2k×]Λk3=E42ΔtΘk4=[c3k×]Λk4=E43ΔtΘk5=I3Λk5=E44+E55+E66+E77Θk6=[Ivn×]Λk6=E56ΔtΘk7=[Ihn×]Λk7=E57ΔtΘk8=exp-αΔtI3Λk8=E88where Eij denotes a 8 × 8 matrix with 1 at position (ij) and 0 elsewhere;
B=[CnkbTI3],Uk=[fkbgn],E=[e5TΔt03×1]and the noise matrix, Wk, is defined as:
Wk=[wknωgΔtnaΔtncΔtncΔtnhΔt]
The process noise covariance matrix is:
Qk=[cov[wk]09×309×309×309×309×303×9σωg2ΔtI303×303×303×303×303×903×3σa2ΔtI303×303×303×303×903×303×3σc2ΔtI303×303×303×903×303×303×3σc2ΔtI303×303×903×303×303×303×3σh21-exp-2αΔt2αI3]where, as defined by [11]:
cov[wk]=((C^nkb)T⊗I3)Lσg2I3LT(C^nkb⊗I3)Δt2
L is the 9 × 3 matrix defined as:
LT=[[e1×][e2×][e3×]]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
Θkr, r = 2, 3, 4, are the functions of elements of
Cnkb. One way of overcoming this difficulty is to substitute
C^nk/kb for
Cnkb in the process equation. This substitution yields a pseudo-linear process equation.
Measurement Model
The matrix measurement equation:
Yk+1=[mk+1cmmk+1]=∑s=12Hk+1sXk+1Gk+1s+Vk+1where the measurement matrices are:
Hk+11=Cbc,Gk+11=[mkn03×105×105×1]Hk+12=I3,Gk+12=[03×1h+bh05×105×1]and the measurement noise covariance matrix is given as:
Rk+1=[σc2I303×303×3σh2I3]
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=18ΘkrX^k/kΛkr+BkUkEΦk=∑r=18[(Λkr)T⊗Θkr]Pk+1/k=ΦkPk/kΦkT+Qk
Measurement update equations:
Y^k+1=∑s=12Hk+1sX^k+1/kGk+1s-Yk+1Hk+1=∑s=12[(Gk+1s)T⊗Hk+1s]Sk+1=Hk+1Pk+1/kHk+1T+Rk+1Kk+1=Pk+1Hk+1TSk+1-1X^k+1/k+1=X^k+1/k-∑j=18∑l=12Kk+1jlY˜k+1Eljwhere ⊗ denotes the Kronecker product [12],
Kk+1jl is a 3 × 3 submatrix of the 24 × 6 matrix Kk+1 defined by:
Kk+1=[Kk+111Kk+112⋮⋮Kk+181Kk+182]and Elj is a 2 × 8 matrix with 1 at position (ij) and 0 elsewhere:
Pk+1/k+1=(I24-KH)Pk+1/k(I24-KH)T+KRKT
Orthogonalization
The Kalman filter is not designed to preserve any relationship among the components of the estimated state matrix
C^nk/kb. 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]:
Cnb∗=C^nb(1.5I3-0.5C^nbTC^nb)
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):
Cnb∗=(C^nbC^nbT)-12C^nb
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)=[cTbgTvnTmvnTmhnTbnT]Twhere c is defined as:
c=[c1c2c3],Cnb=[c1c2c3]
Equation (3) can be rewritten as:
c˙=Ξ(bg)c-Ξ(ωibb)c=Ξ(bg)c+Tωibb
For arbitrary 3 × 1 vector p, we define:
Ξ(p)=[[p×]03×303×303×3[p×]03×303×303×3[p×]],T=[[c1×][c2×][c3×]]
Then, we rearrange the nonlinear kinematic Equations (2), (4), (9), (11), (48) in a pseudo-linear format for computing the Lie derivatives:
[c˙b˙gv˙gm˙vnm˙hnb˙h]=[Ξ(bg)c03×1gn[Ivn×]vn[Ihn×]vn-αbh]︸f0+[T03×303×303×303×303×3]︸f1_ωibb+[03×303×3Cbn03×303×303×3]︸f2_fbwhere
ωibb 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.,
f1_ωibb=f11ωx+f12ωy+f13ωzwhere, 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:
hm=h1∗(x)=Cnb(h+bh)=Ψ(h+bh)cmnc=h2∗(x)=CbcCnbmvn=CbcΨ(mvn)cmhc=h3∗(x)=CbcCnbmhn=CbcΨ(mhn)cwhere:
Cnbp=Ψ(p)c,p=[p1p2p3]TΨ(p)=[p1I3p2I3p3I3]
Furthermore, we enforce the orthogonal constraints by employing the following additional measurement equations:
h4∗(x)=[c1Tc1c2Tc2c3Tc3c112+c122+c132c212+c222+c232c312+c322+c332]-[111111]=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
h1∗,
h2∗,
h3∗, and
h4∗, the ones whose gradients ensure that the observability matrix O is full rank.
The zeroth-Order Lie Derivatives
(𝔏0h1∗,𝔏0h2∗,𝔏0h3∗,𝔏0h4∗): By definition, the zeroth-order Lie derivative of a function is the function itself, i.e.,
𝔏0h1∗=h1∗=Cnb(h+bh)=Ψ(h+bh)c𝔏0h2∗=h2∗=CbcCnbmvn=CbcΨ(mvn)c𝔏0h3∗=h3∗=CbcCnbmhn=CbcΨ(mhn)c𝔏0h4∗=h4∗=[c1Tc1c2Tc2c3Tc3c112+c122+c132c212+c222+c232c312+c322+c332]-[111111]
Therefore, the gradients of the zeroth-order Lie derivatives are exactly the same as the Jacobians of the corresponding measurement functions:
∇𝔏0h1∗=[Ψ(h+bh)03×12Cnb]∇𝔏0h2∗=[CbcΨ(mvn)03×6CbcCnb03×6]∇𝔏0h3∗=[CbcΨ(mhn)03×9CbcCnb03×3]∇𝔏0h4∗=[2M06×15]where:
M=[c11c21c31000000000c12c22c32000000000c13c23c33c1100c1200c13000c2100c2200c23000c3100c3200c33]
The first-Order Lie Derivatives
(∇𝔏f01h2∗,∇𝔏f01h3∗,∇𝔏f1_1h1∗,): The first-order Lie derivatives of
h2∗ and
h3∗ with respect to f0 are computed as:
∇𝔏f01h2∗=∇𝔏0h2∗⋅f0=CbcΨ(mvn)Ξ(bg)c+CbcCnb[lvn×]vn=Cbc[bg×]Cnbmvn+CbcCnb[lvn×]vn∇𝔏f01h3∗=∇𝔏0h3∗·f0=Cbc[bg×]Cnbmhn+CbcCnb[lhn×]vn
Equation (67) is proved in Appendix A, while their gradients are given by:
∇𝔏f01h2∗=[X1-Cbc[Cnbmvn×]CbcCnb[lvn×]Cbc[bg×]Cnb03×6]∇𝔏f01h3∗=[X1-Cbc[Cnbmhn×]CbcCnb[lhn×]03×3Cbc[bg×]Cnb03×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
h1∗ with respect to f̠1, i.e.,
∇𝔏f1_1h1∗. 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):
∇𝔏f1_1h1∗=∇𝔏0h1∗⋅f1_=Ψ(h+bh)T
Stacking the gradients of the three columns together gives:
∇𝔏f1_1h1∗=[Γ09×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+bh)Tei],γi=∂∂bh[Ψ(h+bh)Tei]
Stacking together all the previously computed gradients of the Lie derivatives, we form the observability matrix O as:
𝒪=[∇𝔏0h1∗∇𝔏0h2∗∇𝔏0h3∗∇𝔏f01h2∗∇𝔏f01h3∗∇𝔏f1_1h1∗∇𝔏0h4∗]=[Ψ(h+bh)03×303×303×303×3CnbCbcΨ(mvn)03×303×3CbcCnb03×303×3CbcΨ(mhn)03×303×303×3CbcCnb03×3X1-Cbc[Cnbmvn×]CbcCnb[lvn×]Cbc[bg×]Cnb03×303×3X2-Cbc[Cnbmhn×]CbcCnb[lhn×]03×3Cbc[bg×]Cnb03×3Γ09×309×309×309×3γ2M06×306×306×306×306×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=[Γγ2M06×3]is rank 9 if at least one degree of rotational freedom is excited.
Lemma 2: The 6 × 6 matrix:
B=[-Cbc[Cnbmvn×]CbcCnb[lvn×]-Cbc[Cnbmhn×]CbcCnb[lhn×]]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.
Experimental ResultsHardware 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.
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.
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.
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).
We compute Γ and γ:
Γ=[00000000000h100h200h30-h100-h200-h3000-h100-h200-h3000000000h100h200h3000h100h200h30-h100-h200-h300000000000]123}ωx456}ωy789}ωzγ=[000c31c32c33-c21-c22-c23-c31-c32-c33000c11c12c13c21c22c23-c11-c12-c13000]123}ωx456}ωy789}ωzwhere h + bh = [h1h2h3]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})≠0det(A{2,3,8,10:15})≠0det(A{2,3,6,10:15})≠0
It is worth-mentioning that each selection of the rows is based on two nonzero components of
ωibb=[ω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
ωibb. The rank of A is 9 if at last one degree of rotational freedom is excited.
det(B)=(lh2mh1-lh1mh2)(mv1mh2-mv2mh1)+(lh2mv1-lh1mv2)2mh3where:
lvn=[001]T,mvn=[mv1mv20]Tlhn=[lh1lh20]T,mhn=[mh1mh2mh3]Tdet(B) = 0 when: (a); mh1 = mh2= 0 (b)
lhn and
mvn 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
mvn, the another is linearly independent with
mvn. Then, (b) is not satisfied. So B is full rank if two linearly independent horizontal lines are observed.
Step (1)
Based on Lemma 1, the rank of A is 9. We diagonalize it with the Gaussian elimination:
[Ψ(h+bh)03×303×303×303×3CnbCbcΨ(mvn)03×303×3CbcCnb03×303×3CbcΨ(mhn)03×303×303×3CbcCnb03×3X1-Cbc[Cnbmvn×]CbcCnb[lvn×]Cbc[bg×]Cnb03×303×3X2-Cbc[Cnbmhn×]CbcCnb[lhn×]03×3Cbc[bg×]Cnb03×3I909×309×309×309×309×306×906×306×306×306×306×3]
Then we can easily eliminate all other elements of the first columns:
[03×903×303×303×303×3Cnb03×903×303×3CbcCnb03×303×303×903×303×303×3CbcCnb03×303×9-Cbc[Cnbmvn×]CbcCnb[lvn×]Cbc[bg×]Cnb03×303×303×9-Cbc[Cnbmhn×]CbcCnb[lhn×]03×3Cbc[bg×]Cnb03×3I909×309×309×309×309×306×906×306×306×306×306×3]
Step (2)
We can eliminate all other elements of the fourth and fifth columns using
CbcCnb, respectively:
[03×903×303×303×303×3Cnb03×903×303×3CbcCnb03×303×303×903×303×303×3CbcCnb03×303×9-Cbc[Cnbmvn×]CbcCnb[lvn×]03×303×303×303×9-Cbc[Cnbmhn×]CbcCnb[lhn×]03×303×303×3I909×309×309×309×309×306×906×306×306×306×306×3]
Step (3)
Based on Lemma 2, B is full rank, thus we diagonalize it with the Gaussian elimination:
[03×903×303×303×303×3Cnb03×903×303×3CbcCnb03×303×303×903×303×303×3CbcCnb03×303×9I303×303×303×303×303×903×3I303×303×303×3I909×309×309×309×309×306×306×306×306×306×306×3]
Note that
CbcCnb and
Cnb are both full rank. It is easy to see the matrix is full rank, indicating that the matrix O is also full rank.
ReferencesTittertonD.H.WestonJ.L.Strapdown Inertial Navigation Technology2nd ed.Peter Peregrinus Ltd. on behalf of the Institute of Electrical EngineersLondon, UK2004DiasJ.VinzceM.CorkeP.LoboJ.Special issue: 2nd workshop on integration of vision and inertial sensorsInt. J. Robot. Res.20072651953510.1177/0278364907079279SabatiniA.M.Estimating three-dimensional orientation of human body parts by inertial/magnetic sensingSensors2011111489152510.3390/s11020148922319365WahhaG.A least-square estimate of spacecraft attitudeSLAM Rev.19657409ChoukrounD.WeissH.Bar-ItzhackI.Y.OshmanY.Kalman filtering for matrix estimationIEEE Trans. Aerosp. Elect. Syst.20064214715910.1109/TAES.2006.1603411SabatiniA.M.Kalman-filter-based orientation determination using inertial/magnetic sensors: Observability analysis and performance evaluationSensors2011119182920610.3390/s11100918222163689GrovesP.D.Principles of GNSS, Inertial, and Multi-Sensor Integrated Navigation SystemsArtech HouseNorwood, MA, USA2008FerrarisF.GrimaldiU.ParvisM.Produre for effortless in-field calibration of three-axis rate gyros and accelerometersSens. Mater.19957311330GoddardJ.S.Pose and motion estimation from vision using dual quaternion-based extended Kalman filteringProc. SPIE19973313189200RoetenbergD.LuingeH.J.BatenC.T.M.VeltinkP.H.Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientationIEEE Trans. Neur. Syst. Rehabil. Eng.200512395405ChoukrounD.WeissH.Bar-ItzhackI.Y.OshmanY.Direction Cosine Matrix Estimation from Vector Observations Using a Matrix Kalman FilterProceedings of AIAA Guidance, Navigation, and Control Conference and ExhibitAustin, TX, USA11– 14 August 2003BellmanR.Introduction to Matrix AnalysisMcGraw-HillNew York, NY, USA1960Bar-ItzhackI.Y.ReinerJ.Recursive attitude determination from vector observations: DCM identificationJ. Guid. Contr. Dynam.19847515610.2514/3.56362Bar-ItzhackI.Y.MeyerJ.On the convergence of iterative orthogonalization processesIEEE Trans. Aerosp. Electron. Syst.19761214615110.1109/TAES.1976.308289BroganW.L.Modern Control TheoryPrentice-HallUpper Saddle River, NJ, USA1990HermannR.KrenerA.Nonlinear controlability and observabilityIEEE Trans. Autom. Control19772272874010.1109/TAC.1977.1101601SastryS.Nonlinear Systems: Analysis, Stability, and ControlSpringer-VerlagNew York, NY, USA1999OliensisJ.A new structure-from-motion ambiguityIEEE Trans. Pattern Anal. Mach. Intell.20002268570010.1109/34.865186Figures and Table
Geometry of visual/inertial/magnetic sensor based navigation.
The estimated 3-D trajectory of the pushcart.
The 3σ bounds for the errors in the attitude matrix and the velocities in the n frame.