Kalman filter - help me to set up a state equation

[9988776655]

I would like to find the distance that a vehicle travels using a Kalman filter. The vehicle is a car that travels the road between two positions. The vehicle has a GPS/barometer/accelerometer device that collects position data, which I converted from a longitude and latitude to a North, East, Down position in metres (This is a position coordinate of the starting coordinate is always 0,0,0). The velocity of the car is measured by this device in North, East, Down coordinates (m/s) (components of the velocity vector). I can access accelerometer measurements but this is in the device frame, not the North East Down frame. The device gives magnetic heading and euler angles roll pitch and yaw. The accelerometer has a problem with drift and the magnetic heading is not always accurate so I think that using the accelerometer for observations is not that important. Can anyone suggest a state matrix that helps me get a distance state? Thanks.

 

[timthereaper]

Some questions first:

• How accurately are you trying to get the distance? AFAIK, the max GPS error box (not in altitude) is like 9m, so for a car traveling around a city you'd get pretty close to something actual.
• Is there any reason you can't orient the accelerometers to give readings in the NED frame? That would help with calibration as well as not having to do 3D transformations.
• Are you sure that the accelerometers drift? Because I know gyroscopes do, but in my experience accels are just noisy.
• What device are you using to measure velocity? Accels measure acceleration, which is the derivative of velocity.

I don't think you need the magnetometer either. It's nice sometimes, but it's not really reliable or useful for your application.

You might be able to numerically integrate position from acceleration and correct it with your GPS readings, but that's kinda sketchy because there's error all over the place. If you had some way to measure the input to the wheels (like a u(t) function in the LTI equation xdot=Ax+Bu) and some knowledge about the car like the weight, you might be able to get something more accurate. The best way is if you had some sort of encoder on the car wheels. :)

 

[9988776655]

The distance should just be the best estimation possible, given my observations. I know the GPS can have much error.
The accelerometers can't give readings in the NED frame without transformation using Euler angles. Euler angles are available from the system but I will use the magnitude of acceleration to get around this problem.
The accelerometers are noisy but you are right they don't drift.
The device uses the Doppler effect to measure velocity.
An encoder is not available for use.

I have decided on a Kalman filter model:
http://postimg.org/image/6h29p7e79/

Or for the word file version of this:
http://www.filedropper.com/kalmanfilter_1

The observation for the distance is:
temp(i) = sqrt((yEast(i)-yEast(i-1))^2 + (xNorth(i)-xNorth(i-1))^2+(zDown(i)-zDown(i-1))^2);
distance(i) =distance(i-1) + temp(i);

Where i is the index of a location given by the GPS coordinates in the North East Down frame and i = 0 ... N locations.

Units are metres, metres/sec, metres/sec^2

Can you please give me your thoughts and check if the model can be improved?
Thanks.