Skip to content

Theory

Jump to bottom
Lezh1k edited this page Feb 15, 2018 · 2 revisions

Problem

GPS receivers in some phones may give inaccurate coordinates. Due to this, the positioning error and improper distance calculations occur. There are some solutions below.

Kalman filter

Kalman filtering, also known as linear quadratic estimation (LQE), is an algorithm that uses a series of measurements observed over time, containing statistical noise and other inaccuracies, and produces estimates of unknown variables that tend to be more accurate than those based on a single measurement alone, by estimating a joint probability distribution over the variables for each timeframe.

You can get more details about the filter here.

The filter is a de-facto standard solution in navigation systems. The project simply defines the given data and implements some math.

The project uses 2 data sources: GPS and accelerometer. GPS coordinates are not very accurate, but each of them doesn't depend on previous values. So, there is no accumulation error in this case. On the other hand, the accelerometer has very accurate GPS readings, but it accumulates error related to noise and integration error. Therefore, it is necessary to "fuse" these two sources. Kalman is the best solution here. There are a lot of articles about it.

You can get more details about the accelerometer in the sensor fusion section. Let's assume there is an acceleration axis related to some coordinate system (East/North/Up in our case).

There is a 4-dimensional model with the following parameters measured via GPS: x (longitude), y (latitude), x' (velocity x), y' (velocity y). The model provides us accuracy and is based on NMEA data. To construct the model it is necessary to describe our process.

Let's define matrices:

  1. State transition matrix (F):

[1.0, 0.0, dt, 0.0,

0.0, 1.0, 0.0, dt,

0.0, 0.0, 1.0, 0.0,

0.0, 0.0, 0.0, 1.0] dt here is period between two PREDICT steps.

  1. Observation model (H): Identity matrix 4x4. Coordinates x, y, x' and y' can be got from the GPS receiver.

  2. Control matrix (B) :

[ dt^2 / 2.0, 0.0,

0.0, dt^2 / 2.0,

dt, 0.0,

0.0, dt ] dt here is period between two PREDICT steps.

  1. Control vector (U) : [x'', y''].T - x acceleration (x'') and y acceleration (y'') can be derived with accelerometer and rotation vector.

  2. Process noise (Q) : Q.setIdentity(); Q.scale(accSigma * dt); Looks after that like this :

[ accSigma*dt, 0.0, 0.0, 0.0,

0.0, accSigma*dt, 0.0, 0.0,

0.0, 0.0, accSigma*dt, 0.0,

0.0, 0.0, 0.0, accSigma*dt ] dt here is period between two UPDATE steps. Here predicting error increases through time to compensate inegration error.

  1. Measurement noise (R) : R.setIdentity(); R.scale(posSigma); After that it will looks like :

[ posSigma, 0.0, 0.0, 0.0,

0.0, posSigma, 0.0, 0.0,

0.0, 0.0, posSigma, 0.0,

0.0, 0.0, 0.0, posSigma ]

posSigma is the value got from HDOP or from method Location.getAccuracy.

All formulas derived from movement law : x = x0 + x'*dt + x''*dt^2 / 2 .

Sensor fusion

To get phone's orientation and acceleration axis related to the absolute coordinate system different sensors have to be used. Accelerometer "knows" where is Earth. Magnetometer "knows" where is North. Gyroscope can give angle accelerations with high accuracy. All of them can be used with "sensor fusion", but it's a really complex thing. There are a lot of other solutions: complementary filter, Kalman filter, Madgwick filter, etc. Thanks to Android developers virtual devices LINEAR_ACCELEROMETER and ROTATION_VECTOR with enough precision were created. Madgwick also gives good results and works faster, but it's difficult to find good gain coefficient for that filter. In addition, it has initialization time. There is extended Kalman filter in the core of these sensors. You can get more details here.

A quaternion can be got with ROTATION_VECTOR and can provide rotation matrix. After that, it is necessary to multiply our linear acceleration vector by the rotation matrix and get our acceleration axis as result. Also, it is required to rotate our East/North axis by magnetic declination angle.

So after those operations, acceleration vector related to absolute coordinates appears. It is important that LINEAR_ACCELEROMETER has "drift" and it is necessary to do calibration before using LINEAR_ACCELEROMETER. But the value of this "drift" is not big. About 0.004m/s^2.

Geohash

Geohash is the cool thing that helps us to join several points into one (with some restrictions of course). Geohash can be calculated really fast for the given precision (algorithm looks like a binary search). In the project, it is used for "thinning" coordinates set. For bad signal (+- 300m) it's recommended to use precision 7. Also, it is necessary to count the sum of points with the same geohash. If this sum < 3 (for example) then remove all points related to that geohash.

Problems

  1. Huge integration error related to accelerometer noise. When accelerometer frequency ~50Hz and GPS frequency ~0.5Hz it's ok and nothing to do. With the decrease of GPS frequency, precision gets worse and position is defined wrong. Process noise variance increases with the increase of the period between GPS readings, but this information doesn't seem to be really helpful. It helps when the phone is in the rest state. We can reset filter if this period >= 15sec.
  2. Accelerometer "drift". When a phone is in rest state accelerometer shows some acceleration. About 0.002m/s^2, but it creates significant error. So, it's recommended to make some calibration step.
  3. Process noise covariance doesn't seem to be really precise. It is got by the empiric method. Some information could be found here and Wikipedia, but the result isn't really good with that noise covariance.
Clone this wiki locally