The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences. Vol. XXXVII. Part Bl. Beijing 2008
965
When GPS signals are available, precise GPS positioning
results are used to update the error states in the Kalman filter
The observation vector z includes 6r N , 6r E , 5r D .
(KF1) of 24 states, to estimate the INS errors and to provide
navigation solutions. At the same time, the GPS derived
velocity is used in a second Kalman filter (KF2) of 4 states to
z (0 = — ^gps 1 = t Ax3 ^3x2i ] x g/ (0 + v (0 (7)
estimate the LRF and optic flow modelling errors. The corrected
measurements from the gyro, LRF and optic flow are processed
by the integrated INS/Vision navigation algorithm introduced in
where P /NS and P GPS are the INS and GPS measured positions,
respectively.
Section 2, which estimates horizontal velocity and height above
the ground. During GPS outages, the measurements form LRF,
3.2 Modelling Error Estimation for Vision Sensors
and optic flow and gyro are processed continuously to get
navigation solutions, which are independent from the GPS/INS
solutions.
3.1 Integrated GPS/INS Navigation
The operation of the KF relies on the proper definition of a
dynamic model, an observation model and a stochastic model
As mentioned in Section 3.2, there are several error sources in
the model expressed by Equation (1), for calculating the
platform’s horizontal velocity. The height from the LRF
contains a fixed offset and a scale error. Optic flow has scale
errors in two directions. Therefore, a Kalman filter is designed
to estimate these errors as four states:
(Brown and Hwang, 1997). While the observation model
establishes the relationship between the observations and the
states to estimated, the dynamic model describes the propagation
of system states over time. The stochastic model describes the
x LOF = \8r lb ,8ri f ,8a) fx ,8G)fy\ (8)
stochastic properties of the system process noise and observation
errors.
where 5r| b and 5r| f are the LRF fixed offset and the scale error,
respectively; 5c0fx and 8(0fy are the optic flow scale errors at x
axis and at y axis, respectively.
** = $ *-!**-! + W *-l (2)
z k = H k x k + V k (3)
The dynamic model of these four error states is treated as zero-
mean Gaussian white noise as follows:
where x k is the (nXl) state vector, <X>* is the (nXn) transition
matrix, z k is the (rX 1) observation vector, H* is the (rXn)
observation matrix, w* and v* are the uncorrelated white
X LOF ~ W LOF (9)
Gaussian noise.
The 24 (8x3) Kalman filter error states are:
The observation vector z includes 6v N and 6v E .
X GI Nav X Acc X Gyr0 X Grav X Anl ~\
ZlofW = [V H LO f-V H gJ
=№,JW0+M0 <l0)
where
where V H IN s and V H GPS are the vision and GPS measured
horizontal velocities, respectively.
x /v«v =[Sr N ,Sr E ,Sr D ,Sv N ,Sv E , Sv D , Sy/ N , Sy/ E , Syr D ]
x =rv V V V V V,1
a Acc L v bx’ y by’ y bz’ v fa’ fa’ fa- 1
X Gyro ~ [ £ bx ’ £ by ’ £ bz ] (5)
According to Equation (1), and the four error parameters listed
in Equation (8), the optical flow and LRF navigation error
model is derived as:
X Grav = [ , Sg £ ’ S S D ]
=\si x ,si y ,si z ]
^=[^x(l-ft> Ay )-^]x(^-^)x(l-/7 / ) + ^ (11)
The following complete terrestrial INS psi-angle error model is
adopted in the system:
where e is the bias introduced by the ground slant and other
errors. The Kalman filter error model can then be derived as:
Sit = -(d) ie + co in )x. 8\ - Sxj/ xf + g + V
Sf = -o) en xdr + 8v (6)
Sfr = — 6) in X Sy + £
A W = ( r »-%) x Cl-7/>S»^+ (12)
[Q^ x(1 -co^)-q> v ][(1 -T] f )8rj b + (r ff -7] b )St] f \
(6)
5v g v=( r &- T l b )*Q-Vf)8co ficy +
[Q^ x(1 -co^)-q> v ][(1 -r] f )8r\ b + (r ff -rj b )Srj f \