Is it possible to use other values instead of IMU?

[nuradlinn]

Hi there, I would like to ask if it is possible to convert this Kalman Filter code for positioning. Instead of using IMU values, I want to make x,y coordinate the input.

[xaedes]

Currently as it is, it is just a direct filtering of the the (in this case acceleration) values.
So you could feed it position values instead and it will just filter those.
But the velocity or acceleration of your position inputs won't be tracked.

It uses a Kalman filter, so the Kalman state transition matrix kalman.F and observation matrix kalman.H can be changed.

Too add xy velocity you would need something like this to create and change the kalman filter. It depends on delta time dt and must be updated each time with the actual delta time since last update.

def make_model_xy_constant_velocity():
  return Kalman(4,4)

def update_model_xy_constant_velocity(kalman, dt):
  kalman.F = np.array([
    [1, 0, dt, 0],
    [0, 1, 0, dt],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
  ])
  kalman.H = np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
  ])

If you have only the xy as input it looks like this, but I am not 100% sure right now if this properly tracks the velocity.

def make_model_xy_constant_velocity():
  return Kalman(4,2)

def update_model_xy_constant_velocity(kalman, dt):
  kalman.F = np.array([
    [1, 0, dt, 0],
    [0, 1, 0, dt],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
  ])
  kalman.H = np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0]
  ])

An additional tipp to avoid numerical issues. Call this after update:

kalman.P = (kalman.P + kalman.P.T) * 0.5 + np.eye(kalman.n_states) * 1e-6

This will force the state covariance to be symmetric and helps keeping it in a condition that is necessary for the kalman filter to work properly.
Unfortunately I didn't include this at the time this code was made.
But it helps!

[xaedes]

When you have xy position, velocity and accel as input you can use something like this.

def make_model_xy_constant_accel():
  return Kalman(6,6)

def update_model_xy_constant_accel(kalman, dt):
  kalman.F = np.array([
    [1, 0, dt, 0, dt*dt*0.5, 0], # x = x + dt*vx + 0.5*dt*dt*ax
    [0, 1, 0, dt, 0, dt*dt*0.5], # y = y + dt*vy + 0.5*dt*dt*ay 
    [0, 0, 1, 0, dt, 0],         # vx = vx + dt*ax
    [0, 0, 0, 1, 0, dt],         # vy = vy + dt*ay
    [0, 0, 0, 0, 1, 0],          # ax = ax
    [0, 0, 0, 0, 0, 1]           # ay = ay
  ])
  kalman.H = np.array([
    [1, 0, 0, 0, 0, 0],
    [0, 1, 0, 0, 0, 0],
    [0, 0, 1, 0, 0, 0],
    [0, 0, 0, 1, 0, 0],
    [0, 0, 0, 0, 1, 0],
    [0, 0, 0, 0, 0, 1]
  ])

To change it to 3d just expand the pattern of kalman.F and kalman.H.
With multiple sensors you often actually have different sample rates, that is where it gets complicated to handle correctly...

[nuradlinn]

Thank you very much for the reply!
I have another question, for the line return Kalman(4,2), does this means n_states = 4 and n_sensors = 2?