Kalman filter is a useful tool to calculate the position and the velocity of an object in order to modify in real time a route or to correct an instrumental drift. It allows the update of the least squares estimates if any change happens, as for examples new observations, without needing to solve the whole system. These updates are called epochs.
The updated parameters can be derived from the ones estimated at the previous step plus the increment due to the contribution of the new measurements, as follows:
$$ {\hat{x}_{i + 1}} = {\hat{x}_i} + K({b_{i + 1}} - {A_{i + 1}}{x_i}) $$
(6)
The real unknown of such system is the matrix K that is known as gain matrix or Kalman matrix.
Regarding the Kalman process, it is possible to individuate two different phases: the filtering and the smoothing.
The first phase determinates the best parameter estimate at the current measurement epoch: it has a particular importance in the real-time applications; on the other side, the second phase, starting from the measurement of the last epoch, enables the best estimate of the parameters of the previous epochs and for this reason is not treated in the present paper.
Figure 3 shows the flow chart for the computation of the parameters (hereafter referred as “state vector”) and the variance–covariance matrix at each epoch.
Usually, real-time algorithms foresee a consistency check based on the verification of the position previously calculated using a position, velocity, and time (PVT) computation algorithm. Such check enables the removal of wrong measurement that could affect the system.
When only the position is estimated, the design matrix and the state vector are (Brown and Hwang 1997)
$$ \begin{array}{*{20}{c}} {F = A = \left[ {\begin{array}{*{20}{c}} 1 &; 0 &; 0 \\0 &; 1 &; 0 \\0 &; 0 &; 1 \\\end{array} } \right]} &; {\quad \hat{x} = \left[ {\begin{array}{*{20}{c}} {{x_1}} \\{{x_2}} \\{{x_3}} \\\end{array} } \right]} \\\end{array} $$
where
- (x
1
x
2
x
3):
-
easting, northing, up (height on the ellipsoid) with PVT algorithm
Such a Kalman filter allows an improvement with respect to the solution obtained with the standard PVT algorithm: the consistency check, as told before, allows a reduction of the errors due to noisy measurements, despite the positions that feed the algorithm coming from the same measurement sets (pseudorange and carrier-phase observations) that are affected by the same problems.
Furthermore, the Kalman filtering attenuates the noise that affects the measurements, but it comes at a price: the risk of a wrong solution estimate that can diverge from the true one. This fact has a greater impact if the input data are affected by systematic errors due, for example, to obstacle, as usually happens in the urban environment.
Such problem could be mitigated, taking particular care in the selection of the coefficients of the dispersion matrices, C
ee
and \( {C_{\varepsilon \varepsilon }} \), and setting a check on the predicted residuals between the solutions from filter and PVT algorithm following the expression:
$$ {{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\smile}$}} {v}}_k} = {b_k} - {A_k}{ }{\hat{x}_{k|k}} $$
A possible solution for these problems is represented by the introduction in the filter of a different type of measurement like for example the Doppler shift observable.
The state matrix, the state vector, and the design matrix with the introduction of these new variables become:
$$ \begin{array}{*{20}{c}} {F = \left[ {\begin{array}{*{20}{c}} 1 &; 0 &; 0 &; {\Delta t} &; 0 &; 0 \\0 &; 1 &; 0 &; 0 &; {\Delta t} &; 0 \\0 &; 0 &; 1 &; 0 &; 0 &; {\Delta t} \\0 &; 0 &; 0 &; 1 &; 0 &; 0 \\0 &; 0 &; 0 &; 0 &; 1 &; 0 \\0 &; 0 &; 0 &; 0 &; 0 &; 1 \\\end{array} } \right]} &; {\hat{x} = \left[ {\begin{array}{*{20}{c}} {{x_1}} \\{{x_2}} \\{{x_3}} \\{{{\dot{x}}_1}} \\{{{\dot{x}}_2}} \\{{{\dot{x}}_3}} \\\end{array} } \right]} &; {A = \left[ {\begin{array}{*{20}{c}} 1 &; 0 &; 0 &; 0 &; 0 &; 0 \\0 &; 1 &; 0 &; 0 &; 0 &; 0 \\0 &; 0 &; 1 &; 0 &; 0 &; 0 \\0 &; 0 &; 0 &; 1 &; 0 &; 0 \\0 &; 0 &; 0 &; 0 &; 1 &; 0 \\0 &; 0 &; 0 &; 0 &; 0 &; 1 \\\end{array} } \right]} \\\end{array} $$
where
- x1, x2, x3:
-
easting, northing, and up (from PVT algorithm)
-
\( {\dot{x}_1},\,{\dot{x}_2},\,{\dot{x}_3} \)
:
-
velocity in the east, north, and up directions (from PVT algorithm)
- Δt
:
-
GPS sampling interval
The observables and the state and variance/covariance matrices can be assumed equal to the values obtained from the position and velocity PVT estimation algorithm.
The measurement dispersion matrix C
ee
, in particular, contains the inverse of the weights to be attributed to the various elements of the state matrix during the Kalman filter process. These quantities depend on the coordinate estimation algorithm used before the Kalman filter stage.
Concerning the system dispersion matrix \( {C_{\varepsilon \varepsilon }} \), the values of the matrix coefficients are related to both the characteristics of the GPS signal and the problem set in the Kalman filter. In particular, in this case was chosen a matrix \( {C_{\varepsilon \varepsilon }} \) derived from that proposed for a similar problem, within (Brown and Hwang 1997):
$$ C_{\varepsilon \varepsilon }^{\rm{UTM}} = \left[ {\begin{array}{*{20}{c}} {{S_p}\frac{{\Delta {t^3}}}{3}} &; 0 &; 0 &; {{{\dot{S}}_p}\frac{{\Delta {t^2}}}{2}} &; 0 &; 0 \\0 &; {{S_p}\frac{{\Delta {t^3}}}{3}} &; 0 &; 0 &; {{{\dot{S}}_p}\frac{{\Delta {t^2}}}{2}} &; 0 \\0 &; 0 &; {{S_h}\frac{{\Delta {t^3}}}{3}} &; 0 &; 0 &; {{{\dot{S}}_h}\frac{{\Delta {t^2}}}{2}} \\{{{\dot{S}}_p}\frac{{\Delta {t^2}}}{2}} &; 0 &; 0 &; {{S_p}\Delta t} &; 0 &; 0 \\0 &; {{{\dot{S}}_p}\frac{{\Delta {t^2}}}{2}} &; 0 &; 0 &; {{S_p}\Delta t} &; 0 \\0 &; 0 &; {{{\dot{S}}_h}\frac{{\Delta {t^2}}}{2}} &; 0 &; 0 &; {{S_h}\Delta t} \\\end{array} } \right] $$
(7)
where:
- S
p
, S
h
:
-
spectral width associated with the white noise caused by the guide and related to the planimetric (S
p
) or altimetric (S
h
) coordinates in Universal Transverse Mercator (UTM) system
-
\( {\dot{S}_p},{\dot{S}_h} \)
:
-
spectral width associated with the white noise caused by the guide and related to the planimetric \( \left( {{{\dot{S}}_p}} \right) \) or altimetric \( \left( {{{\dot{S}}_h}} \right) \) velocities in UTM system
- Δt
:
-
GPS sampling interval
Note how the planimetric coordinates have been weighted differently if compared with the height and with the three velocities. We have adopted, in particular, planimetric values higher than altimetric ones because, during the road tests, the planimetric coordinates vary more than the altitude.
This dispersion matrix is defined in a local projection or in a UTM projection, whereas coordinates and velocities calculated from the pseudorange and Doppler shift measurements are expressed in the ECEF frame. For this reason, the final system dispersion matrix will be the result of the propagation of the matrix \( C_{\varepsilon \varepsilon }^{\rm{UTM}} \) with the derivative of the transformation matrix \( R_{\rm{ECEF}}^{\rm{UTM}} \)between the two reference systems:
$$ R_{\rm{ECEF}}^{\rm{UTM}} = \left[ {\begin{array}{*{20}{c}} { - \sin \lambda } &; {\cos \lambda } &; 0 &; 0 &; 0 &; 0 \\{ - \sin \varphi \cos \lambda } &; { - \sin \varphi \sin \lambda } &; {\cos \varphi } &; 0 &; 0 &; 0 \\{ - \cos \varphi \cos \lambda } &; { - \cos \varphi \sin \lambda } &; { - \sin \varphi } &; 0 &; 0 &; 0 \\0 &; 0 &; 0 &; { - \sin \lambda } &; {\cos \lambda } &; 0 \\0 &; 0 &; 0 &; { - \sin \varphi \cos \lambda } &; { - \sin \varphi \sin \lambda } &; {\cos \varphi } \\0 &; 0 &; 0 &; { - \cos \varphi \cos \lambda } &; { - \cos \varphi \sin \lambda } &; { - \sin \varphi } \\\end{array} } \right] $$
It is then possible to calculate the measurement dispersion matrix \( C_{\varepsilon \varepsilon }^{{\rm{ECEF}}} \) with the variance/covariance propagation law:
$$ C_{\varepsilon \varepsilon }^{\rm{ECEF}} = R_{\rm{ECEF}}^{\rm{UTM}} \cdot C_{\varepsilon \varepsilon }^{\rm{UTM}} \cdot {\left( {R_{\rm{ECEF}}^{\rm{UTM}}} \right)^T} $$
(8)