Getting zero in velocities

[antonagafonov]

Hi, I am measuring 7 measurements which are x and estimating both x and x_dot. While i am getting good Kalman filtering on the measured vector i am getting 0 for with x_dot.

Can someone point where i am missing?

Thanks

Here is my implementation.

` class Kalman(): def init( self, dim_x = 14, X0 = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.], p=1, r=100, q=1, dt = 1/10): # observation of position (xyz, qw qx qy qz), estimation of velocity and position self.f = KalmanFilter(dim_x=dim_x, dim_z=int(dim_x/2))

        # initial state (location and velocity)
        self.f.x = np.array([X0]).T

        # state transistion matrix
        self.F = np.array([
                        [1., dt, 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0. ],
                        [0.,  1., 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.],
                        [0.,  0., 1., dt,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0. ],
                        [0.,  0., 0.,  1.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.],
                        [0.,  0., 0.,  0.,  1., dt,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0. ],
                        [0.,  0., 0.,  0.,  0.,  1.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.],
                        [0.,  0., 0.,  0.,  0.,  0.,  1., dt,  0.,  0.,  0.,  0.,  0.,  0. ],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  1.,  0.,  0.,  0.,  0.,  0.,  0.],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  0.,  1., dt,  0.,  0.,  0.,  0. ],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  0.,  0.,  1.,  0.,  0.,  0.,  0.],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  1., dt,  0.,  0. ],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  1.,  0.,  0.],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  1., dt ],
                        [0.,  0., 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  1.]
                    ])

        # measurement function x y z qw qx qy qz only not the derivatives
        self.f.H = np.array([
                        [1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
                        [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
                        [0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
                        [0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0.],
                        [0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.],
                        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.],
                        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.]
                            ])
        # P variance of prior
        self.f.P = np.eye(dim_x)*p
        # multiply 0,2,4,6,8,10,12 by h 
        # h = 10
        # for i in [0,2,4,6,8,10,12]:
        #     self.f.P[i,i] = self.f.P[i+1,i+1]*h

        # R variance of measurement
        self.f.R = np.eye(int(dim_x/2))*r
        
        # Q variance of process
        self.f.Q = np.eye(dim_x)*q

        self.estimates_x = []
        self.measurment_x = []
        self.ps = []

def kalman_step(self,xz):
    self.f.predict()
    self.f.update(xz)
    self.estimates_x.append(self.f.x.flatten().tolist())
    self.measurment_x.append(xz.flatten().tolist())
    estimated = self.f.x.flatten()
    xz_estimated = estimated[[0,2,4,6,8,10,12]]
    vz_estimated = estimated[[1,3,5,7,9,11,13]]
    print(self.f)
    return xz_estimated, vz_estimated 

`