159
P(* +11 k +1) = [I - Uk +1) • H] P(* +11 k) ■...
[l-L(t + l) H] + L(k + 1) ■ R ■ L(k +1)'
(16)
where in (11) S(k + 1 I k) is the new estimate at step
(k+1), as derived by update S(k I k) at time step k and
discretized function f k (•) (see 4.1), obtained from
nonlinear relationship (6) without error w(t); in (12)
P(fc + 1 I k) is the covariance matrix of a priori predi
ction error, O(klk) is the fundamental matrix related to
linear system =F(s(i))-<^ and Q(t) is the cova
riance matrix of discretized error w(t), both reported in
subsection 4.2; in (14) L(k+1) is the gain matrix of the
filter; in (15) S(k + 1 I k + 1) is the new update at time
step (k+1); finally in (16) Y*(k + 1 I k + 1) is the a
posteriori estimate error covariance.
4.1 PREDICTOR DISCRETIZATION.
In the prediction step, unlike the EKF theory, which
states to solve the differential equation
s(i)=f(s(i)) s(t k ) = s(k\k) (17)
to compute the predictor
s(k + \ \ k) = s(t k+] ) (18)
we employ the discrete scheme below, which yields the
discretized form in (11) of the predictor itself:
s(k +1) = fj (s(k)) = s(k) + (t k+] -t k )-...
(t -t ) 2 • ( 19 )
•f(s(^))+ u+, 2
4.2 CALCULATION OF O(klk) AND Q(t).
If we define the Jacobian matrix
(20)
J=s(t)
for t=tk, assuming s(t k ) = s(k I k) , then F(klk)=
F(S(k I k) ) and performing a discretization we obtain
O(k I k) = exp{(^ +1 -t k )-F(k I k)} (21)
As regards Q(t), the covariance matrix of error w(t)
assumes following form:
Q(*) = (i* +1 -t k )-Q>(k I k)-Q-®(k I k)' (22)
5. COORDINATES TRANSFORMATION.
Our main purpose for realizing such a mobile mapping
system consists to carry out tipically road surveys on
behalf of various italian public administrations or
private agencies which are interested in road data
collection for GIS applications. To this aim in most
cases we will have to deal with cadastral maps that in
Italy involve two different cartographic projections:
Gauss-Boaga and Cassini-Soldner. The first is based on
Hayford ellypsoid, oriented in Roma-Monte Mario,
while the second refers to Bessel ellypsoid oriented in
Genova: these are the two national geodetic datums,
commonly used for mapping applications. Therefore
prior to use the corrected GPS positions, as measure
ments input for the Kalman filter, we perform a geode
tic datum transformation between WGS-84 and one of
our national datums, employing the Wingeo software
developed on the basis of algorithms proposed by [1].
6. TEST AND RESULTS.
In order to test the accuracy of the positioning module,
i.e. the goodness of our kinematic model so as the
effectiveness of our implementation of Kalman filter,
we have simulated a survey employing random gene
rated true position data with Gaussian noise with the
same variance error as our GPS receiver (single fre
quency Trimble Pro-XR) according to its technical data
sheet (50 cm RMS, adopting Everest 1 M multipath
rejection technology).
Comparing these true positions with that computed by
the filter, 86% of angle estimates move less than 2
degree away from true value, while 50% present a
discrepancy limited to 1 degree (Fig. 8).
On the basis of these results and positioning accuracy
of our receivers, as guaranteed by Trimble, we could
conclude that our NavPos software module provides a
positional error about 30-35 cm for object at distance
of 10 m from the vehicle.
Bearing error
Fig. 8: Simulation results on bearing error
F(s(i)) =
ds
