Background : To give everyone some background I am trying to smoothen GPS data received from a device using Kalman filter (Apache Common implementation). What kind of dynamic noises should I include in my implementation with respect to matrix P0, Q and R knowing that the only input I have apart from position(X and Y) is horizontal accuracy and velocity for X and Y components. This is not a constant velocity example since the velocity might change from one ping to another ping.
Implementation Library : Apache Common
- http://commons.apache.org/proper/commons-math/userguide/filter.html
Usage : I am only considering 2D space for now
Inputs that I have with me :
1. latitude
2. longitude
3. horizontal accuracy or Horizontal Dilution of precision (HDOP) in meters/sec
4. Time between two pings (dt) = 30 seconds
Outputs I care about
1. new Latitude
2. new Longitude
Calculated values : Vx (velocity in X direction) Vy (velocity in Y direction) The object will be constantly moving but with varying velocities so I can calculate Vx and Vy using the formulate V * sin(theta) and V * Cosine(theta)
How should I map my values to the Apache Common implementation.?
Current setup :
X = Initial State = [
{X Y X-Vel Y-Vel}
]
// I only care about X and Y coordinates so this is a 2 * 4 matrix
H = Observation variables = [
{1, 0, 0, 0},
{0, 1, 0, 0}
]
// This is a 4 * 4 matrix
P0 = Cov(X) = [
{(horizontal accuracy from i/p), 0, 0, 0},
{0, (horizontal accuracy from i/p), 0, 0},
{0, 0, (some initial value for VY), 0},
{0, 0, 0, (some initial value for VX) }
]
// Copied this from somewhere. What values should I have in this? //
This is a 4 * 4 matrix
Q = Cov(Dynamic noise) = [
{ Math.pow(dt, 4d)/4d, 0d, Math.pow(dt, 3d)/2d, 0d },
{ 0d, Math.pow(dt, 4d)/4d, 0d, Math.pow(dt, 3d)/2d },
{ Math.pow(dt, 3d)/2d, 0d, Math.pow(dt, 2d), 0d },
{ 0d, Math.pow(dt, 3d)/2d, 0d, Math.pow(dt, 2d) }
]
// This is a 2 * 2 matrix
R = Cov(measurement noise) = [
{ Math.pow((horizontal accuracy from i/p), 2d) , 0},
{ 0, Math.pow((horizontal accuracy from i/p), 2d)}
]
// This is a 4 * 4 matrix
A = State transition matrix = [
{ 1d, 0d, dt, 0d },
{ 0d, 1d, 0d, dt },
{ 0d, 0d, 1d, 0 },
{ 0d, 0d, 0d, 1d }
]
Are my matrices correct for what I am trying to do? When I run them I keep getting MatrixDimensionMismatchException and hence I decided to ask a question. Any help would be appreciated.
I was able to solve this problem by using the latest version of Apache Commons Math library.
It turns out that Apache Commons Math 3.2 and earlier version had a major bug
reported here : https://issues.apache.org/jira/browse/MATH-1033
The problem was measNoise column dimension always had to be 1 which means matrix R should always have only 1 column
// row dimension of R must be equal to row dimension of H
if (measNoise.getRowDimension() != measurementMatrix.getRowDimension() ||
measNoise.getColumnDimension() != 1) {
throw new MatrixDimensionMismatchException(measNoise.getRowDimension(), measNoise.getColumnDimension(), measurementMatrix.getRowDimension(), 1);
}