Chapter 4. Inertial and optical sensor fusion
Figure 4.1 — Inertial navigation system (INS). The position and orientation in the global frame is computed using the sensor signals from the IMU. The upper input is the accelerometer signal a − g, the lower input is the angular velocity from the gyroscopes ω.
The discrete Kalman filter assumes the vector of states being estimated, xt, evolves according to a state propagation equation or dynamic model:
xt+1 = Axt+ wt (4.2)
where A is the state transition matrix from t to t + 1, and that measurements zt are related to the states by a linear measurement model:
zt= Cxt+ vt (4.3)
where wtand vtrepresent process and measurement noise with covariance matrices EwtwTt = Qtand EvtvtT = Rt, respectively [35]. Note, that in a complemen-tary filter structure the states xt are error states, therefore, they can be read as xε,t. Figure 4.3 gives a complete picture of the operation of the Kalman filter [114].
The outputs of the filter are used to correct the position, velocity, acceleration and orientation estimates ˆp+ ,ˆv+ ,ˆa+ and ˆΘ+.
The most important factors contributing to the output error are incorporated in the error state vector:
xε,t= [pε,t, vε,t, Θε,t, aε,t, ωε,t, qε,t]T (4.4) with pε,t, vε,t and Θε,t being the position error, velocity error and orientation error, respectively, aε,t is the accelerometer error and ωε,t is the gyroscope error and qε,t is the position error from the optical system. All state vectors represent 3D vectors:
xε= xε,x xε,y xε,z
(4.5)
52
4.2. Design of the fusing filter
Figure 4.2 — Complementary Kalman filter structure for position and orientation estimates combining inertial and Vicon measurements. The signals from the IMU (a − g and ω) provide the input for the INS (Figure 4.1). By double integration of the acceleration, the position ˆp− is estimated at a high frequency. At a feasible lower frequency, the optical tracking system provides position q. The difference between the inertial and optical estimates z is delivered to the Kalman filter. Based on the system model the Kalman filters estimates the propagation of the errors ˆxε. The outputs of the filter are used to correct the position, velocity, acceleration and orientation estimates ˆp+ ,ˆv+,ˆa+ and ˆΘ+.
4.2.3 Error model
The discrete inertial error model with timestep ∆t, follows directly from the system description in Figure 4.1. The position error is calculated by the integration of the velocity error. The velocity error is the integration of the acceleration error. The velocity error is also depending on the orientation error multiplied by the measured acceleration signal as follows from Equation 4.1. The orientation error can be found by taking the first order approximation of a strapdown integration step:
pε,t+1 = pε,t+ ∆tvε,t (4.6)
vε,t+1 = vε,t+ ∆t G(at− g) × Θε,t+ aε,t
(4.7)
Θε,t+1 = Θε,t+ ∆t [ωε,t×] (4.8)
where the matrix cross product operator is given by:
[ω×] =
0 −ωz ωy ωz 0 −ωx
−ωy ωx 0
(4.9)
53
Chapter 4. Inertial and optical sensor fusion
Figure 4.3 — Kalman filter loop. The a priori states ˆx− and error covariance matrices P− are predicted each time step t. When the measurement z comes available, the Kalman gain K is computed and the a posteriori estimates ˆx and P are computed after which the process can be repeated [114].
The acceleration and gyroscope errors aε,t and ωε,t are modeled as first order Markov processes.
aε,t+1 = aε,te−βa∆t (4.10)
ωε,t+1 = ωε,te−βω∆t (4.11)
The autocorrelation of the Markov process is defined by [16]:
RX(τ ) = σ2e−β|τ | (4.12)
The optical position error qε,t is also modeled as a first order Markov process with additional white measurement noise:
qε,t+1 = qε,te−βq∆t+ vq,t (4.13)
In most optical systems, the correlation in errors between two consecutive samples from the optical system will be low and the error can be described just by the white noise term. The state transition matrix At is defined from equations 4.6 to 4.13:
At=
I3 ∆tI3 0 0 0 0
0 I3 ∆t [(a − g) ×] ∆tI3 0 0
0 0 I3 0 [∆t×] 0
0 0 0 e−βa∆tI3 0 0
0 0 0 0 e−βω∆tI3 0
0 0 0 0 0 e−βq∆tI3
(4.14)
54
4.2. Design of the fusing filter
where I3 is a three by three identity matrix and 0 a three by three matrix of zeros.
In this system, it is assumed that the noise for each state variable is uncorrelated with the noise for each other state. Hence, all non-diagonal terms of the noise matrix Qt matrix are zero and the diagonal terms are the variances of the random variables.
4.2.4 Measurement model
The measurement presented to the Kalman filter is the distance measured by the optical system qt minus the inertial distance estimate pt (see Figure 4.2).
Therefore, the discrete measurement model is formed from the inertial position error pε,t and optical error qε,t :
Ct = I3 0 0 0 0 I3
(4.15) The Rtparameter is the variance associated with the white measurement noise vt term in Equation 4.3. The noise of the sensors in one direction is assumed to be uncorrelated with the sensor noise in another direction. Therefore, the non-diagonal elements of the measurement covariance matrix Rt matrix are zero.
4.2.5 Smoothing
In an off-line analysis, which is often used in clinical applications, the whole mea-surement sequence from z1 to zN is available for processing. This offers valuable in-formation about the error state propagation and enables the use of a fixed-interval Kalman smoothing algorithm. The principle of the fixed-interval smoothing algo-rithm during an optical gap is illustrated in Figure 4.4 [46]. The figure shows that two separate inertial solutions computed in the forward and backward directions quickly increase over time. When the Kalman smoothing algorithm is applied to the data, the error is significantly reduced across the data gap interval.
The Rauch-Tung-Striebel (RTS) algorithm provides an efficient method for implementing the Kalman filter smoothing [35]. The first (forward) sweep uses the Kalman filter as described previously but saves the computed a priori estimates ˆ
x−t and a posteriori estimates ˆxt and their associated matrices P−t and Pt at each step time t. The second pass runs backward in time in a sequence from the time tN of the last measurement, computing the smoothed state estimate ˆx[s]t from the intermediate results stored on the forward pass. The recursive equations for the backward sweep are:
ˆ
x[s]t= ˆxt+ St ˆx[s]t+1− ˆx−t+1
(4.16) where the smoothing gain St is given by:
St = PtATtP−t+1−1 (4.17) 55
Chapter 4. Inertial and optical sensor fusion
Figure 4.4 — Kalman filter smoothing during gap. The inertial errors increase rapidly over time when no reference data is available. By smoothing the forward and backward filtering, the mean square errors are significantly reduced.