Note: Descriptions are shown in the official language in which they were submitted.
CA 02716597 2010-10-08
r Y
Apparatus and Method for Driftless Attitude Determination and Reliable
Localization of Vehicles
Field of the Invention
This invention relates to the field of robotics, and more particularly to the
driftless attitude
determination and reliable localization of vehicles, such as mobile robots,
waking robots, or
humanoid robots.
Background of the Invention
Both position and attitude determination of a mobile robot are necessary for
navigation, guidance
and steering control of the robot. Dead-reckoning using vehicle kinematic
model and incremental
measurement of wheel encoders are the common techniques to determine the
position and
orientation of mobile robots for indoor applications. However, the application
of this technique for
localization of outdoor robots is limited, particularly when the robot has to
traverse an uneven
terrain or loose soils. This is because wheel slippage and wheel imperfection
cause quick
accumulation of the position and attitude errors.
The problem with inertial systems is that they require additional information
about absolute
position and orientation to overcome long-term drift. An attitude estimation
system based on
utilization of multiple inertial measurements of a mobile robots is known.
Only pitch and roll angles
may be estimated in this method by using gravity components deduced from
measurements of two
accelerometer, but the yaw angle is not detectable. An integrated inertial
platform consisting of
three rate gyros, a triaxial accelerometer and two tilt sensors is known to
minimize long-term drift.
Other research focuses on using vision system as the absolute sensing
mechanism required to
update the prediction position obtained by inertial measurements.
1
CA 02716597 2010-10-08
Attitude determination GPS (ADGPS) using a multi-GPS antenna technology, in
which the receiver
gives not only the position but also the velocity data, are used for airborne
applications as well as
marine vessels and ships. Integration of such a GPS system and Inertial
Navigation System (INS) is
straightforward because attitude measurements from the two systems is directly
available. ADGPS
however need large antenna separation, e.g., 17 m, for offering competitive
accuracy for azimuth
and pitch determination of a ship. Multiple GPS antennas with separation
distance of 11 m has been
tried for attitude determination of aircraft. Methods for attitude estimation
of UAVs based on
position and velocity information obtained from a single GPS antenna have been
disclosed. Since the
velocity information is obtained from the Doppler shift measurement of the GPS
carrier signals,
these methods are suitable for fast moving vehicles such as UAVs, but not for
mobile robots or
humanoid robots.
Nowadays, differential Global Navigation Satellite Systems (GNSSs) to
centimeter-level accuracy are
commercially available making them attractive for localization, guidance and
control of outdoor
mobile robots. GNSS is the generic term for a satellite-based navigation
system, such as the Global
Positioning System (GPS), the Russian GLONASS, or European Galileo systems,
and the term will be
used in this specification to encompass all such systems. It will also be
understood that in some
environments, such as on mars, a satellite-based system may not be available.
However, in such
environments, it is possible to replace the satellites by transmitters placed
at fixed locations that are
functionally equivalent to satellites. Such transmitters are known as
"pseudolites", and it will be
understood that the expression GNSS as used herein will also encompass such
pseudolite-based
systems. However, for convenience the invention will be described specifically
in the context of the
GPS system, which is the most widely used system today.
2
CA 02716597 2010-10-08
In conventional GPS, the pseudo ranges to satellites are measured by aligning
the locally generated
pseudorandom signal to the received signal. In Real Time Kinematic (RTK) GPS,
and more specifically
RTK GPS, an attempt is made to align the carriers rather than the signals
modulated on to the
carriers. Since the carriers are about 1000 times faster than the signals,
considerable improvements
in accuracy over conventional GPS can be achieved.
One method of improving the accuracy of localization systems using RTK GPS in
the presence of GPS
latency is to use a localization algorithm based on Kalman filtering that
tries to fuse information
coming from an inexpensive single GPS with inertial data and map-based data. A
decentralized data
fusion algorithm for simultaneous position estimation of a land vehicle and
building the map of the
environment by incorporating data from inertial sensor, GPS, laser scanner,
the wheel and steering
encoders is described in J. Vaganay, M.J. Aldon and A. Fournier, "Mobile robot
attitude estimation
by fusion of intertial data", IEEE Int Conference on Robotics & Automation,
Atlanta Georgia, May
1993, pp 277-282.
The use of vision systems has also been investigated. However, vision systems
have proven to be
sensitive to the lighting condition of the environment.
Summary of the Invention
According to a first aspect of the present invention there is provided a
method of determining
positional information about a vehicle, comprising: computing estimates of the
covariance matrices
of Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS)
measurement data obtained
by at least two GNSS receivers mounted on the mobile vehicle; and fusing the
RTK GNSS
measurement data according to their corresponding covariance matrices to
obtain enhanced
positional information.
3
CA 02716597 2010-10-08
Embodiments of the invention provide a method for estimating vehicle attitude
and position, in
three dimensions, by optimally fusing two RTK GNSS measurements,
accelerometric measurements
of gravity from an accelerometer (or an inclinometer instrument) and angular
rate measurements
from a rate gyro. The relation between the GPS noises and the difference
between the measured
and actual antenna-to-antenna baselines is developed that allows to estimate
the covariance matrix
of the GNSS measurement noises.
First, the covariance matrices associated with the two RTK GNSS measurements
are estimated based
on the error on the magnitude of the antenna-to-antenna baseline measurement
and the
confidence of the GNSS receivers on the horizontal and vertical axes
measurements. Then, the
observation vector is constructed from the measurements of the onboard
accelerometer and two
RTK GNSSs. The measurement of the onboard rate gyro, the discrete-time
observation as well as the
estimated measurement covariance matrices are used by an Extended Kalman
filter for driftless
attitude determination of the mobile robot. Finally, the estimated attitude
(in term of quaternion),
RTK GPS measurements and the estimated covariance matrixes are used by another
estimator to
optimally localize the mobile robot by taking advantage of the redundancy in
the GPS
measurements plus the knowledge of the GNSS noise characteristics. This allows
enhancing the
accuracy and robustness of the GNSS-based localization of vehicles.
Observability analysis shows that driftless attitude estimation requires the
line connecting the
antennas not to be collinear with the vector of gravity.
According to a second aspect of the invention there is provided an apparatus
for determining
positional information about a vehicle, comprising: at least two GNSS
receivers for mounting on the
vehicle robot and defining an antenna-to-antenna baseline therebetween; and a
processor
4
CA 02716597 2010-10-08
configured to compute estimates of the covariance matrices of Real Time
Kinematic (RTK) Global
Satellite Navigation System (GNSS) measurement data obtained by at least two
GNSS receivers
mounted on the vehicle, and to fuse the RTK GNSS measurement data according to
their
corresponding covariance matrices to obtain enhanced positional information.
A further aspect of the invention provides an apparatus for determining
positional information
about a vehicle, comprising at least two GNSS receivers for mounting on the
vehicle and defining an
antenna-to-antenna baseline therebetween; an inertial Measurement Unit (IMU)
for obtaining data
about the movement of the mobile robot; and a processor configured to and to
fuse the RTK GNSS
measurement data and the data from the IMU to obtain enhanced positional
information.
Brief Description of the Drawings
The invention will now be described in more detail, by way of example only,
with reference to the
accompanying drawings, in which: -
Figure 1 is a block diagram of an apparatus in accordance with one embodiment
of the invention;
Figure 2 shows a mobile robot with two GPS antennas and an IMU mounted on the
robot;
Figure 3 shows a mobile robot with three GPS antennas and an IMU mounted on
the robot for
experimental purposes;
Figure 4 shows the path taken by an experimental robot;
Figure 5 shows the IMU outputs;
Figure 6 shows the RTK GPS outputs;
Figure 7 shows the confidence on the RTK GPS measurements;
Figure 8 shows the estimated parameters;
CA 02716597 2010-10-08
Figure 9 shows the vehicle attitude;
Figure 10 shows the vehicle position;
Figure 11 shows the attitude errors; and
Figure 12 shows the position errors.
Detailed description of Embodiments of the Invention
The apparatus shown in Figure 1 comprises a vehicle 1 with wheels 2. A pair of
RTK GPS antenna 3
coupled to respective GPS receivers 4, which may be differential receivers,
are mounted a fixed
distance apart on the vehicle 1 and separated by an antenna-to-antenna
baseline 5. The baseline 5
should not be collinear with the vector of gravity for driftless attitude
estimation. An accelerometer
6 and rate gyro 7 are mounted on the mobile robot 1. The embodiment will be
described in
connection with GPS, although as noted the invention applies more generally to
any GNSS system,
including systems using fixed pseudolites instead of satellites.
The mobile robot 1 also includes a processor 10, which may be a general-
purpose computer,
comprising a plurality of software/and or hardware modules, namely a
covariance estimation
module 11, a stochastic estimator module 12, an observation vector module 13,
and an extended
Kalman filter module 14, and an initializer 18.
The signals from the receivers 4 are subtracted from each other in subtraction
module 15.
It will be understood that the various modules can be implemented in software,
hardware or a
combination of the two.
A base antenna 16 is also provided at a fixed location as is known in RTK GPS.
6
CA 02716597 2010-10-08
The method of estimating robot attitude and position, in three dimensions,
involves optimally fusing
two RTK GPS measurements, accelerometric measurements of gravity from an
accelerometer (or an
inclinometer) and angular rate measurements from a rate gyro in the Kalman
filter 14 (KF). In
addition to the pose, the KF 14 also estimates the gyro bias.
RTK GPS devices notoriously suffer from signal robustness issue as their
signal can be easily
disturbed by many factors such as satellite geometry, atmospheric condition
and shadow. To deal
with the uncertain GPS noise problem, the covariance matrix of the GPS noises
is estimated in real-
time so that the KF filter 14 is continually "tuned" as well as possible.
Kinematics
Figure 2 schematically illustrates a vehicle 1 as a rigid body to which
multiple differential GPS-
antennas 3 and an IMU device are attached. Coordinate frame {A} is an inertial
frame while (B) is
a vehicle-fixed (body frame) coordinate system. The origin of frame {A}
coincides with that of the
base antenna of the differential GPS system, i.e., the GPS measurements are
expressed in this
frame. On the other hand, the origin of {B} coincides with the IMU center and
their axes are
parallel, i.e., the IMU measurements are expressed in (B). The orientation of
{B} with respect to
]T, {A} is represented by the unit quaternion q = C qqo where subscripts v and
o denote
the vector and scalar parts of the quaternion, respectively. The rotation
matrix A representing the
rotation of frame {B} with respect to frame {A) is related to the
corresponding quaternion q by
A(q) = (2q2 o -1)13 +2g0[gvx]+2q qv, (1)
7
CA 02716597 2010-10-08
where [., ] denotes the matrix form of the cross-product. Consider quaternions
and q3.
Then, A(q3) = A(q,)A(q,) and q3 = q2 q, are equivalent, where denotes the
quaternion-
product and the operators [q ] is defined, analogous to the cross-product
matrix as
[q ]= T - diag aq,, x],0) where =[ q0 q,
-qv q
The conjugate q' of a quaternion is defined such that q` q = q q* = [0 0 0
If.
Now, assume that vector r represents the location of the origin of frame {B}
that is expressed in
coordinate frame {A}, and p; is the output of the Z th GPS measurement.
Apparently, from Fig. 2,
we have
p; = r+A(q)e; +vp1 . `di =1,2 (2)
where constant vectors e,s are the locations of the GPS antennas in the
vehicle-fixed frame and
vpi s represent the GPS measurement noises, which are assumed random walk
noises with
covariance E[vp vT.] = Rp .
1 1 1
The IMU 20 is equipped with an accelerometer which can be used for
accelerometric measurements
of gravity. In general, accelerometer output contain components of the
acceleration of gravity and
the inertial acceleration. In mobile robots, the level of inertial
acceleration is negligible compared to
the acceleration of gravity; typically maximum inertial acceleration is about
0.02g. Therefore,
despite the fact that estimation of the inertial acceleration of the robot can
be obtained from the
GPS data, it is sufficient to model the inertial acceleration as a measurement
noise in the KF. Let
assume that a be the accelerometer output. Then, the acceleration equation can
be written as
8
CA 02716597 2010-10-08
a
Ilall = A T k + v, (3)
where II=II denotes the Euclidean norm, and unit vector k is defined to be
aligned with the gravity
vector in frame {A}, while va captures the accelerometer noise and inertial
acceleration all
together. We treat va as a random walk noise with covariance E[vaVa VT] = 213'
Observation Equations
The objective of the extended Kalman filter (EKF) is to determine the vehicle
attitude and position
by fusing the IMU and GPS measurements.
In principle, the attitude of a rigid body can be determined from two non-
collinear position vectors
each of which expressed each in two coordinate systems. Equation (3) gives the
gravity vector in
both frames, while the baseline vector ApSp, -p2 is the rotated version of the
vector from one
antenna to the other, i.e., e, -e2 . Therefore, equations (2) and (3) suffice
to obtain the attitude q
and the position r. However, this will lead to an inaccurate estimate because
of low signal-to-noise
ratio of the Op measurement. Denoting vectors Ae = el -e2 and vp = Vp2 - Vp1 ,
one can infer
from (2) that
IlAell = IIAp+v,, 11 = IIApI +Ap Vp, (5)
where
p= ~p
~~ p11
9
CA 02716597 2010-10-08
and the RHS of (5) is obtained by the first-order approximation of the norm of
summation of two
vectors. Antenna-to-antenna baseline IlDell is approximately 1 m, whereas the
GPS error isabout
t5 Cm. This means that the error in measurement of orientation of vector op is
about 10%,
which is far from desired accuracy. The two GPS observations in conjunction
with the measurements
of the acceleration gravity are used as external updates in an elaborate
Kalman filter integrating a
rate gyro data with the observation data.
In the described embodiment, the IMU signals are given at the rate of 20 Hz,
whereas the GPS data
can be acquired at the rate of 1 Hz. Therefore, an average of the IMU signal
can be obtained
between two consecutive GPS data acquisitions. This tends to decrease the IMU
noise. Therefore,
the discrete-time measurement of the acceleration can be obtained by averaging
through
integration of the IMU signals at that interval tk -te < t<- tk , where to
denotes the GPS sampling
rate, i.e.,
ak - t f la(~)dc (5)
e
Therefore, the discrete-time measurement vector is defined as
zk k k (6)
ak/llakll
In view of (2), (3) and (6), the observation vector as a function of the
discrete-time variables qk and
kk can be written as
h A(gk )(el - e2) (7)
k AT(gk)kk
CA 02716597 2010-10-08
r
Thus, the observation equation is
zk = hk + Vk, where Vk = [VP'k - vp2k
Vak
is the overall additive measurement noise. Assuming that the noises of the two
GPS receivers are
not mutually corrected and that they are also not corrected with the IMU
noise, the covariance
matrix of the measurement noise takes the form
R 'k 03 /
Rk = E[Vkvk ] = 2 ' (8)
03 a13
where R = R + R and R = E[v vT ] is the covariance matrix associated with the
Pk Pik P2k Pik Pik Pik
measurement nose of the : th GPS.
The discrete-time observation vector (7) is a nonlinear function of the
quaternion. To linearize the
observation vector, one also needs to derive the sensitivity of the nonlinear
observation vector with
respect to the system variables. To this end, consider small orientation
perturbations
&1=q qi= (9)
around a nominal quaternion 4. Now, by virtue of A(q) = k(,5q (8> q), one can
compute the
observation vector (7) in terms of the perturbation 8q. Using the first order
approximation of
nonlinear matrix function AT (Sq) from expression (1) by assuming a small
rotation sq, i.e.,
1l8q = 1 and &7o =1, i.e.,
A(Sq) =13 +2[89,,x].
11
CA 02716597 2010-10-08
To take the decomposition rules of quaternion into account, the state vector
to be estimated by the
EKF is defined as
x =col(Sgv,b) (10)
where vector b is the corresponding gyro bias. Thus, the sensitivity matrix
can be written as
H _ ah _ -2A[(e1-e2)x] 03 11
ax 2AT [kx]A 03 , ( )
where A=A(4) Thus, the linearized observation equation is
Zk =H%k+Vk (12)
Covariance Estimation of the GPS Error
Efficient implementation of the KF requires the statistical characteristics of
the measurement noise.
The IMU noises are not usually characterized by time-invariant covariance.
Therefore, 6Q is a
constant parameter, which can be either derived from the sensor specification
or empirically tuned.
The GPS noise characteristics, however, are time-varying; the errors vary
depending on many factors
such as satellite geometry, atmospheric condition, multipath areas, and
shadow.
The covariance of the GPS antenna-to-antenna measurements can be estimated
from the difference
between IIApII and IIAeII, which are both the antenna to antenna baselines
expressed in two
different frames. The magnitude of a vector is invariant under rotation
transformation, i.e., ideally
IIApII = IIieII if GPS data is noise-free. However, from the error equation
(4), we can infer that
12
CA 02716597 2010-10-08
E[(IIAPkII-IILeI)2]=ApkTRpkAPk= (13)
If the covariance matrix is assumed diagonal as
R Pk 13, (14)
pk Pk
then a'2 =E[(II/pII-IIAeI)2]. Moreover, since GPS receivers estimate the
position via the
averaging process over the sampling interval, we can assume E[IIAPII] =
IIAPkII = Therefore, knowing
that Le is a deterministic variable, we can find the covariance from
6pk = (I kpk l -Del )2 .
Most Real Time Kinematic (RTK) GPS receivers can estimate the confidence on
their position
measurements in real-time that allows us to characterize the covariance matrix
more accurately.
Typically, the positions of horizontal axes are measured with the same
confidence, while the height
measurements are given with less confidence. Therefore, for the i-th GPS, we
can say
C; =diag(ch 1 .,ch 1 .,c,,), where ch and cv. are the confidence on the
horizontal and vertical
7 1
measurement data that are provided by the receiver in real-time. We assume
that
Rpi = pC2
i di =1,2, (15)
where the scaling factor p is yet to be found. Substituting (15) in the RHS of
(13) yields
(IILPk II - IIAeII)2
P OPT (C k +C2k )ePk
Thus, the covariance matrix of the GPS noise can be computed from
13
CA 02716597 2010-10-08
1 = 4
R - _( epkll- jAell)2 C2; .
p~k 1X (C k +C2k )epk k
Attitude Estimation
The relation between the time derivative of the quaternion and the angular
velocity w can be
readily expressed by
. (16)
q= 2w q where w=
]
The angular rate obtained from the gyro measurement is
cog =w-b-Eg (17)
where b is the corresponding bias vector; Eg is the angular random walk noises
with covariances
E[EgE;] = a213 . The gyro bias is traditionally modeled as
b = Eb, (18)
where Eb is the random walk with covariances E[EbEb ] = 6b 13 . Then, adopting
a linearization
technique one can linearize (16) about the nominal quaternion q and nominal
velocity
w=cog+b,
to obtain
d8q =-wx8q +bb+IE . (19)
dt v v 2 2 g
14
CA 02716597 2010-10-08
R 4 4
Since &q,, is not an independent variable and it has variations of only the
second order, its time
derivative can be ignored. Setting the dynamics equations (19) and (18) in the
standard state space
form, gives
-t x = Fx+GE, (20)
where t =col(Eg,Eb); and
F= -[6)X] 113 G= 213 03
03 03 03 13
Observabilty Analysis
A successful use of Kalman filtering requires that the system be observable. A
linear time-invariant
(LTI) systems is said to be globally observable if and only if its
observability matrix is full rank. The
observability matrix associated with linearized system (20) together with the
observation model (11)
is
r
O=[ HT (HF)T ... (HFS )T I .
The states of the system are assumed to be completely observable if and only
if
rank 0 = 6 (21)
which is equivalent to 0 having 18 independent rows.
Although the described system is not time-invariant as F and H are defined on
the nominal
trajectory at a frozen instant in time, examination of the LTI observability
measure can still provide a
CA 02716597 2010-10-08
useful insight into the local observability of the system at that time. To
this end, we construct the
submatrices of the observability matrix 0, i.e.,
BF = [_A[Lex1[ix1 2 A[Dex]wx] 2 A[k'x] (22)
where k' = ATk is the unit gravity vector expressed in the vehicle frame and
we used the identity
AT[kx]A = [k'x] to obtain (22). The observability matrix can be reduced to the
following block-
triangular matrix by few elementary Matrix Row Operations (MRO)
MRo M 03
O - O'= x , (23)
where
M = [k'x] [Dex] - [iex] [k'x] (24)
and hence rank 0 =rank 0'. It is clear from (23) that the full rankness of the
block-triangular
matrix rests on the invertibility of the square matrix M. In other words, if M
is invertible, then the
system is observable.
Proposition 1 If a line connecting the two GPS antennas is not collinear with
the gravity
vector, then system (12)-(20) is observable.
Proof: In a proof by contradiction, we show that ME R3x3 must be a full-rank
matrix. If M
is not full-rank, then there must exist a non-zero vector tl ~ 03x1 such that
Mrs = 03x1 (25)
In view of definition (24) and the property of the vector product of three
vectors {a,b,c},
16
CA 02716597 2010-10-08
a x (b x c) =(a = c)b - (a = b)c,
one can easily show that LHS of (25) is equivalent to
Mn = k'x (Ae x q) - Ae x (k'x q)
= (k'. i)Ae - (De = i)k' # 0, (26)
in which, the inequality of (26) is inferred from the fact that vectors k' and
De are not parallel and
hence they can not annihilate one other. Therefore, it is not possible for
(25) to be true meaning
that matrix M must be full rank.
Discrete Time Model
The equivalent discrete-time model of (20) is
Xk+1 = (bkxk +Wk (27)
where Fk = t1(tk +te,tk) is the state transition matrix over time interval t.
. In order to find a
closed form solution for the state transition matrix, we need the nominal
values. The nominal values
of the relevant states at interval tk <T <tk +te are obtained from the latest
estimate update, i.e.,
qk(T) = qk, "Jr) = bk. Similar to (5), the nominal value of the angular
velocity is obtained by
averaging through integration of the IMU signals at that interval between two
consecutive GPS data
acquisition, i.e.,
wk = bk + 1 jrk Wg(~)dd. (28)
to 'k-1
Therefore, the nominal angular velocity remains constant in the interval.
Under these
circumstances, the state transition matrix can be obtained in closed form for
constant angular
17
CA 02716597 2010-10-08
velocity or for varying angular velocity remains constant in direction. Let us
define Ok(a) = WkZ and
9k = IIAkIl . Then, the state transition matrix Dk(T) = 4)(z,tk +T) takes on
the form:
fik (Z) _ Tlk (Z) 2 T2k (Z) (29)
0 13
where the elements of the above matrix are given as
17lk(te)=13-Sln9k [ekx]+1-COSOk [0kx]2 (30)
9k 9k
T2k (te) = (13 + COS92 -1 [Okx]+ 9k -sin [ekX]2)to,
9k 9k
Let E. =blockdiag(6g13,6b 13) be the continuous-time covariance matrix of the
entire process
noise. Then, the corresponding discrete-time covariance matrix is
Qk = E[Wkwk] = 1`k+t,k~(t)GEG T(DT
(t)dt, (31)
rk
which has the following structure
Qk = Xlk QA21 (32)
b A 3
For small angle 9k, where SlnOk = 9k - 6 9k and cosOk = 1-29k , the elements
of the state-
transition matrix (30) can be effectively simplified. Then, upon integration
of the substitution of the
simplified state transition matrix into (31) we arrive at
18
CA 02716597 2010-10-08
r -`
AI -(62to+6bt )13+6bt [ekx]2+(6gte+~btA)[0 ]4
k 4 12 240 80 1008
Azk = 6b t2 (113 - 1 [Okx] + 1 [OkX]2 ).
4 12 48
Estimator Design
The state transition matrix (29) is used only for covariance propagation,
while the system states
have to be propagated separately by solving their own differential equations.
Let us compose the
redundant state aX =col(q,b), which contains the full quaternion q and
parameters b. Combining
(16), (18), we then describe the state-space model of the system as
az=f(az,E)
The EKF-based observer for the associated noisy discrete system (27) is given
in two steps: (i)
estimate correction
Kk = Pk HT (HkPk HT +Rk)-1 (33)
1k = 1k +K(Zk -h(xk)) (34)
Pk = (112 -KkHk)Pk (35)
and (ii) estimate propagation
rr
ai +I =a 1k + J k+rof (ax(t),O)dt (36)
rk
Pk+l - 0kPk(Dk + `tk (37)
19
CA 02716597 2010-10-08
The vector of discrete-time states, %k, which is to be estimated by the KF,
contains only the
variations Sgvk not the quaternion qk. However, the quaternion can be obtained
from the former
variables if the value of the nominal quaternion 4k is given, that is
84k = qk qk (38)
A natural choice for the nominal value of quaternion is its update estimate as
4(tk-1) = 4k-1 = Since
the nominal angular velocity Wk is assumed constant at interval tk-1 <_ t:5
tk, then, in view of (16),
the nominal quaternion evolves from its initial value 4(tk-1) to its final
value 4k = q(tk) according
to:
leek
qk =e2 qk-1= (39)
It can be shown that the above exponential matrix function has a closed-form
expression so that
the above equation can be written as
qk = (Cos 0k + sm 9k )qk-1 + (? sin Bk - 1CosBk )6k (&k-1 (40)
2 2 9k 2 2 2
From (38), the KF step, i.e., (34), can be written in terms of the full
quaternion, qk instead of its
variation Sqvk , as
SQ,k = [vec(fG k)]K(zk -hk) (41)
Pk Pk
where 4k is obtained from (40). Finally, assuming that IlSgvk I) < 1, a valid
unit quaternion can be
constructed from
CA 02716597 2010-10-08
k k qk = [_11~'kI12 g(tk) (42)
Substituting (40) into (42) gives
"`lvk
qk 1 - j Cos B+sin Bk )Qk-1 +e sin Bk - 2 Cos Bk )0k qk-1
Il8vk 112 k
(Note the last 9 in the above equation should be in bold)
Initialization of the Kalman Filter
For the first iteration of the EKF, an adequate guess of the initial states is
required. The best guess
for the parameters at t = 0 s is = 03, while the initial orientation of the
vehicle with respect to the
inertial frame at t = 0 s is not known beforehand. It is that presumed
IIBq,,II <_ 1. If this condition is
violated, the error quaternion will not be unit norm. To prevent this from
happening, it is important
to keep the initial error in quaternion estimate small as much as possible.
Mathematically, the rotation matrix can be computed from two non-collinear
unit vectors k and
Op and their rotated versions a ma/IIalI and Ai 5. Similar to the methods for
registration of 3-D
laser scanning data, let us form the following matrices from the units vectors
as
D. = [k Op kxAP] (43)
Db = [a 05 "x051 (44)
Then, in the absence of measurement noise, i.e., v = 0, the above matrices are
related by
Da = ADb (45)
21
CA 02716597 2010-10-08
Matrices Da and Db are non singular as long as k and Op are not collinear,
i.e., the line
connecting the GPS antennas is not parallel to the gravity vector. Then, under
this circumstance, the
rotation matrix can be obtained from
A = D,,Db' (46)
Solution (46) yields a valid rotation matrix A so that ATA =13 , only if there
is no error in the
column vectors of matrices (45). However, due to the IMU and GPS noises, a
valid rotation matrix
may not be obtained from solution (46). To correct this problem, one may
observe that all singular
values of any orthogonal matrix must be one. This means that the singular
value decomposition of
the RHS of (33) yields
DaDb' = UEVT ,
where U and V are orthogonal matrices and matrix E = 13 +A1 is expected to be
close to the
identity matrix, i.e., IIAEII < 1. Therefore, by ignoring small matrix A,:, a
valid solution to the
rotation matrix can be found as
A = UVT, (47)
which then can be used to obtain the equivalent quaternion at t = 0 s.
Position Estimation
With the estimation of attitude in hand, one can use the two sets of position
and orientation
equations (4) to compute r. Despite an estimate of r can be obtained from one
set of equations, a
better estimation can be achieved by solving two equations together if the
measurement noise
characteristic is known.
22
CA 02716597 2010-10-08
Setting the two GPS equations (4) in the matrix form, we get
y = Lr+np +nq (48)
none
where
jpi_Aei , L= [13 ,
P2 -Ae2 13
while GPS measurement errors and attitude estimation errors are respectively
represented by
vectors
v _ [elx]
n p = V PI
1 P2 PI nq = -2A I[e2X] Sqv
Using the argument that mutual correlation between np and nq is negligible, we
can write the
covariance matrix of the entire noise in (48) as:
R, = E[npnp]+E[ngnT ] (49)
Rpl +4A[elx]Pq[elx]AT 4A[elx]Pq[e2x]AT
4A[e2x]Pq[elx]AT Rp2 +4A[e2x]Pq[e2x]AT
where Pq = E[64,,64 ]E R'. Since the Kalman filer gives the covariance matrix
of all estimated
states including the quaternion, one can get Pq from the filter covariance
matrix as
E f~ p 6x6.
P = [Pq X
X x
23
CA 02716597 2010-10-08
Then an optimal solution to (48) can be obtained by the weighted pseudo
inverse where a suitable
weighting matrix is the covariance matrix of the noise. That is
(LTR;'L)-'LTR~y, P, -Aei (50)
P2 - Ae2
Using (48) in (50) yields
(LTRr'L)-'LTR,'
If the orientation estimation error is sufficiently small, i.e., the second
term in RHS of (49) is
negligible, then (50) can be conveniently written as
i = W,(P~ -Ae,)+W2(P2 -Ae2),
where the weighting matrices are defined as
W, LRp2 (Rpl +Rp2 )-' (52)
W2SSRpl (Rpl +Rp2 )-'. (53)
It is worth noting that W1 + W2 = 13. This means that the estimator (51)
proportionally puts more
weight on that GPS measurement with the smallest covariance matrix in order to
estimate the
position.
Experimental Verification
The test vehicle was equipped with three RTK GPS antennas as shown in Figure
3. Although only two
GPSs are required by the adaptive KF, having three GPSs allowed measurement of
the vehicle
24
CA 02716597 2010-10-08
4
attitude purely from a kinematic relation. Despite its poor accuracy, the
three-GPS attitude
determination method does not introduce any attitude drift. Therefore, it was
possible to
investigate whether or not the pose estimation method based on fusing two GPSs
and an IMU
exhibits any drift.
Assume that p3 and e3 denote the third GPS measurement and the location of its
antenna on the
vehicle, respectively. Also, denote Op' = P1 _P3 and De'= el -e3 and
N. = [Op Op' Ap x Op']
Nb = [De De' AexAe'] (54)
where identity Na = ANb holds in the absence of GPS measurement noise.
Therefore, in a
development similar to (40) -(47), one can calculate the rotation matrix from
the above matrices.
A rover equipped with three RTK GPS receivers along with satellite antennas
and radio modems
(model Promark3RTK from Magellan Navigation Inc.) in addition to an INIU
device from Crossbow
Technology, Inc. (model IMU300) was used for experiments. Experiments were
conducted on the 30
x 60 m Mars Emulation Terrain (MET) of the Canadian Space Agency. The
locations of the GPS
antennas expressed in the vehicle-frame are shown in the following table. The
additional GPS
receiver was provided to permit estimation of the vehicle pose indpendently of
the IMU
measurements.
CA 02716597 2010-10-08
Table
x(m) y(m) z(m)
el -0.452 0.604 -0.252
e2 -0.452 -0.616 -0.224
e3 -0.427 -0.003 -0.249
An operator sent a pre-scheduled sequence of primitive commands to the mobile
robot-e.g., "move
forward of a certain distance", "rotate clockwise by 45 ", etc-so that the
robot follows a preplanned
path going through some specified via points. Fig. 4 shows the 3-D path taken
by the mobile robot.
The time-histories of the IMU measurements and GPS measurements are depicted
in Figs. 5 and 6,
respectively, while the time-history of the confidence on the GPS data is
shown in Fig. 7.
The Two-GPS-IMU fusion method was compared with One-GPS-IMU method. Although
it was not
possible to obtain ground truth attitude and position information during the
test, it was possible to
obtain an estimate of the attitude without any drift and a fairly reliable
estimate of position by
processing the data acquired from the three GPS receivers.
Fig. 8 shows that the Kalman Filter can quickly identify the parameters, i.e.,
gyroscope bias and the
direction cosines of the gravity vector in the inertial frame. Trajectories of
the vehicle position and
attitude based on the Two-GPS-IMU fusion method and One-GPS-IMU method versus
the results of
the geometric pose estimation process based on the data of the three GPS
receivers (used as the
reference) are plotted in Figs. 9 and 10, respectively. The errors between the
estimated and the
reference pose trajectories are calculated by
Position Error= rref II
26
CA 02716597 2010-10-08
Orientation Error = 2 sin' Ilvec (q of O 411
and the results are plotted in Figs. 11 and 12. It is clearly evident from
Fig. 11 that the attitude
estimation error using one GPS and IMU accumulates gradually due to gyro
drift, whereas the Two
GPS-IMU method exhibits no drift. As shown in Fig.12, the position estimation
based on one GPS
also exhibits drift, which is the echoed attitude drift. Furthermore, it is
apparent from the figure that
the position estimation based on one GPS measurement is not accurate at the
time around t = 400 s,
because the error margin of the first GPS receivers during that period is
incidentally high, as shown
Fig. 6. Both drift and spike in the position errors are substantially reduced
in the Two-GPS-IMU
estimation method.
The described system can be used for outdoor mobile robots for terrestial
applications and to
provide a ground-truth pose estimation of a vehicle during testing. The system
is also applicable to
applications on other worlds, such as the Moon or Mars, where low power
pseudolites can be
substituted for orbiting satellites.
27
