'r 
V ’ 
V 
= 
a 
o, 
r (t) = {x(t),y(t),z(t)} (4) 
This model can be applied in different operating 
modes, employing as state vectors whether pseudoran 
ge or carrier phase osservables, as collected by only 
one receiver or in differential mode. Note that (4) 
represents a costant acceleration model, in which 
vectors v and a incorporates the linear and angular 
components. 
In order to take into account the error introduced by 
our unique positioning device (GPS) we use following 
formula: 
s(t)=f{s(t)-t}+w(t) (5) 
where 
s(t) is the state vector; 
f(t) describes mathematically the nonlinear 
relationship between parameters in s(t) 
and the process; 
w(t) models sensor (rover receiver) syste 
matic errors, which are considered as 
white Gaussian noise with covariance Q. 
In order to determine the components of s(t), kinematic 
measurements are employed according to the formula 
below: 
z* =H-s(i) + n* (6) 
where 
z k measurements vector collected at discrete 
time t k ; 
H matrix of measuring states; 
n k measurement Gaussian error with 
covariance R. 
cp(t) angle of tangent to trajectory at time t; 
co(t) angular velocity of the van; 
v(t) linear velocity; 
a(t) linear acceleration; 
wl(t), w2(t) Gaussian 2D positional error 
components. 
Since we are interested in planimetrie survey, we 
consider only the (X,Y) GPS coordinates as computed 
by differential correction in post-processing, therefore 
the matrix form of equation (6) becomes: 
k* =X k +n hk 
[^2,* — yk y n 2,k 
(8) 
where 
Z] k ) z 2 , k components ov measurements vector 
at step k; 
n i,k> n 2 , k Gaussian components of measurement 
stochastic errors vector. 
In this way the matrix of osservable states assumes 
following form: 
0 0 
1 0 
0 0 0 
0 0 0 
0\ 
being the state vector 
s(i)={x(i) y{t) (pit) 00(f) v(t) a{t) ait)} 
(9) 
00) 
4. STATE VECTOR ESTIMATE. 
Since the Extended Kalman Filter (EKF) represents the 
optimal estimate procedure in case of uncorrelated 
measurements and Gaussian noise with null average, 
the employed algorithm can be summarized by follo 
wing formulas: 
In more detail our nonlinear kinematic model is 
composed by following 7 differential equations: 
x(t) = vit)-COSi(p) 
yit) = v(0 • sen(<p) 
(pit) = coit) 
< (bit) = ait) (7) 
v(t) = ait) 
ait) = 0 + w, it) 
ait) = 0 + w 2 it) 
where 
x(t), y(t) 2D vehicle coordinates on mapping 
frame E m ; 
• Prediction step, 
s(k + \\k) = ft(s(k\ky) (ll) 
P(* + ll/t) = <P(k\k) ■ P(k\k) ■ ®(k\k) +Q(k) (12) 
• Update step, 
'A(k + 1) = H • P(k + 1 lit) • H'+R (13) 
Uk +1) = P(k +11 *) • H'-A(* + 1)“' (14) 
s(fc + llfc + l) = s(fc + llfc) + L(fc + l)-.... 
■[z(k + \)-U-s(k + l\k)] U5)