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 \