How Can a Kalman Filter Estimate Distance Using GPS and Velocity Data?

[9988776655]

I want to get the distance between points. I have a measurement of latitude, longitude and height from GPS/barometer systen (which I converted into north east down coordinates) and I have another measurement of the vehicle's velocity in the north east and down direction. To find the distance, I want to set up the state space equation for a Kalman filter. I attached a picture of my attempt. In the picture, Ts is the sample period, v_k is the magnitude of the velocity, p_k is the position and D is the distance. Please correct my program.

 

[Stephen Tashi]

v_k is the magnitude of the velocity, p_k is the position and D is the distance. Please correct my program.

Explain your notation or give a link to an example that explains it. You also need to explain the problem completely in words. All that's clear from your verbal description is that there is a moving object. The Kalman filter requires that you specify the physical dynamics of a situation not merely the set of measurements. Is this a falling object? - a projectile? - something self propelled?

I can't interpret your state variables. Are you using the magnitude of velocity, but not it's 3 component vectors? Do you mean p_k to be a scalar? Or does it stand for 3-D column vector with 3 entries? What distance is "the distance"?

Since your matrix implies D[k] = D[k-1] + P[k-1], perhaps the coordinates of the object are D[k] and P[k] is a displacement vector instead of a vector of giving the coordinates of the object.?

My recollection of questions about Kalman filters in the math sections is that they often go unanswered and I myself don't claim to be an expert on them. You might have better luck in the engineering sections of the forums. But even there, you have to explain enough so the physical dynamics are known. Just listing your set of measurements isn't sufficient.

 

[9988776655]

I posted this in the General Engineering forum without my state space equation (which is wrong anyway)

 

[Stephen Tashi]

Explain the physics and your notation and I'll do what I can.

 

[Stephen Tashi]

I'll suggest a simple approach. Something is changing the car's velocity, but you haven't mentioned any control inputs. So assume the accelerations are from random noise. That would explain the velocity changes.

Let \triangle t be the time interval between measurements.

State variables:

Let the cars position coordinates be (X[k],Y[k],Z[k]) at measurement k.

Let V_x [k-1] be the average x-velocity of the car between the k-1 and k measurements.
Let A_x[k-1] be the average x-acceleration of the car between the k-1 and k measuremens.Dynamics:

X[k] = X[k-1] + V_x[k-1] * \triangle t + (1/2) * A_x[k-1] * (\triangle t)^2

V_x[k] = V_x[k-1] + A_x[k-1]*\triangle t

Similarly, for the other coordinates:

Y[k] = Y[k-1] + V_y[k-1]* \triangle t + (1/2)* a_y[k-1] *(\triangle t)^2

Z[k] = Z[k-1] + V_z[k-1] *\triangle t + (1/2)*a_z[k-1] *(\triangle t)^2

V_y[k] = V_y[k-1] + A_y[k-1]*\triangle t

V_z[k] = V_z[k-1] + A_z[k-1]*\triangle t

 

[9988776655]

Hi. Thanks for your advice. I posted something very similar to this in the general engineering forum where I explained my notation:
https://www.physicsforums.com/threads/kalman-filter-help-me-to-set-up-a-state-equation.785573/
My version is slightly different because I assumed constant Jerk. I also integrated to get velocity and position, but I used the magnitudes as it was only important to find the magnitude of distance.
Since the object is a car, the accelerations are from putting the pedal down and braking is from using the brakes. How would you model that? Do you think my model is OK?

 

[Stephen Tashi]

You still haven't defined what you mean by "distance". Do you mean distance to be what the odometer on the car measures? -, which is the length of the possibly curved path the car has followed..

 

[9988776655]

Yes that is what I want to measure