# Kalman filter - help me with the state space equation

Tags:
1. Dec 3, 2014

### 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.

#### Attached Files:

• ###### state space.jpg
File size:
13 KB
Views:
128
Last edited: Dec 3, 2014
2. Dec 3, 2014

### Stephen Tashi

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.

3. Dec 3, 2014

### 9988776655

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

4. Dec 3, 2014

### Stephen Tashi

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

5. Dec 4, 2014

### 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$

6. Dec 7, 2014

### 9988776655

Hi. Thanks for your advice. I posted something very similar to this in the general engineering forum where I explained my notation:
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?

7. Dec 7, 2014

### 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..

8. Dec 7, 2014

### 9988776655

Yes that is what I want to measure