rlabbe / filterpy

Python Kalman filtering and optimal estimation library. Implements Kalman filter, particle filter, Extended Kalman filter, Unscented Kalman filter, g-h (alpha-beta), least squares, H Infinity, smoothers, and more. Has companion book 'Kalman and Bayesian Filters in Python'.
MIT License
3.22k stars 612 forks source link

Kinematic Filter has same variance in all dimensions #285

Closed AlexAtCCRI closed 1 year ago

AlexAtCCRI commented 1 year ago

i expect that kinematic filter run on random data will have different values for the variance of each position coordinate. the following example shows numerical equality between the variances for the x,y, and z. components. am i missing something?

      import  numpy as np 
      from filterpy.common import kinematic_kf,kinematic_state_transition,Saver

      order = 2
      n     = 100 
      kf    = kinematic_kf(dim=3, order=order, dt=1)
      s     = Saver(kf)

      t  = np.linspace(0,100,n)
      r  = np.random.rand(n*3).reshape((n,3))
      dt = np.concatenate([[0], np.diff(t)]) 

      kf.x  = kf.H.T@r[0] # set initial position to first meas
      t_old = t[0]

      for this_t,this_dt,this_r in zip(t,dt,r):
          kf.predict(kinematic_state_transition(dt=this_dt,order=order))
          kf.update(this_r)
          s.save()

      assert(P[0,0] ==P[3,3]==P[6,6]) # i thought this should fail. 

      P= s.P[50]# inspect it 
AlexAtCCRI commented 1 year ago

nevermind this. it is my misunderstanding of kalman.