CODEWITHSUNDEEP
positionkalman-filterestimation
Alexandros Mel
Alexandros Mel

Reputation: 55

Use Kalman Filter to estimate position

I try to use Kalman filter in order to estimate the position. The input in the system is the velocity and this is also what I measure. The velocity is not stable, the system movement is like a cosine in general. So the equation is: xnew = Ax + Bu + w, where:

x= [x y]'
A = [1 0; 0 1]
B= [dt 0; 0 dt]
u=[ux uy]
w noise

As I mentioned, what I measure is the velocity. My question is how would the matrix C look like in the equation:

y= Cx + v

Should I involve the velocity in the estimated states (matrix A)? Or should I change the equations to involve also the acceleration? I can't measure the acceleration.

Upvotes: 1

Views: 1348

Answers (1)

Malcolm
Malcolm

Reputation: 752

One way would be to drop the velocities as inputs and put them in your state. This way, your state is both the position and velocity and your filter uses as observation both the measured speed of your vehicle and a noisy estimate of your position.

With this system your problem becomes:

x = [x_e y_e vx_e vy_e]'
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]
w noise

with x_e, y_e, vx_e, and vy_e the estimated values of the state

B is removed because u is 0. And then you have

y = Cx + v
with C = [1 0 0 0 ; 0 1 0 0 ; 0 0 1 0 ; 0 0 0 1]

With y = [x + dt*vx ; y + dt*vy ; vx ; vy] and x, y, vx, and vy the measured values of the velocities and x and y the position calculated with the measured velocities.

It is very similar to the example you will find here on Wikipedia

Upvotes: 0

Related Questions