Simple usage of Kalman Filter

[brunoeducsantos]

Hi, I integrate this package as follows:

from filterpy.kalman import KalmanFilter
import numpy as np
from filterpy.common import Q_discrete_white_noise

class KFMapAlignment:
    def __init__(self,initial_pose,covariance_factor,noise_factor):
        self.f = KalmanFilter (dim_x=3, dim_z=3)
        #set initial pose
        self.x= initial_pose.T
        #set state transition matrix F 
        self.F=np.eye(3)*1.
        #set measurement function
        self.H=  np.eye(3)*1.   
        #set covariance matrix(3,3)
        self.P =np.eye(3)*covariance_factor
        # set  measurement noise
        self.R= np.eye(3)*noise_factor
        #set process noise
        self.Q= Q_discrete_white_noise(dim=3, dt=0.1, var=0.13)

    #measurement: dx,dy,dtheta map alignment correction
    def sensor_fusion_data(self,z):
        self.x, self.P= self.f.predict(z,self.P,self.F,self.Q)
        self.x, self.P = self.f.update(self.x, self.P, z, self.R, self.H)
        return self.x

And after use it in a loop with data as follows:

initial_pose= np.array([corr_angle,x_trans,
kalman= kf.KFMapAlignment(initial_pose,P,R)

  for ...:

                    z= np.array([corr_angle,x_trans,y_trans])
                    new_pose=kalman.sensor_fusion_data(z)

And I am getting the following error:

    # x = Fx + Bu
    478         if B is not None and u is not None:
--> 479             self.x = dot(F, self.x) + dot(B, u)
    480         else:
    481             self.x = dot(F, self.x)

<__array_function__ internals> in dot(*args, **kwargs)

TypeError: can't multiply sequence by non-int of type 'float'

What is my issue?

Regards, Bruno

[rlabbe]

I don't understand your code. The kalman filter class maintains its own x, P, Q, etc. You are storing them in your own class. And then when you call predict and update you are passing in entirely wrong values. For example, you pass z into predict Predict does not take a measurement. Read the docstrings for predict and update, you'll see not only that you are passing in the wrong things, but that what you are trying to do isn't possible. You can pass the state and covariance into these functions, because they are internally held.

Please read the header of kalman_filter.py. There are two examples of nearly what you are trying to do. The first one uses the KalmanFilter class, and the second one uses the stand alone functions that require you to maintain the states. I kind of think you were attempting the latter, but for some reason trying to use the KalmanFilter class.

[brunoeducsantos]

Thanks @rlabbe . Indeed, I need an Extended version of Kalman Filter and save the previous state. I need to correct my motion equations to include relation between [x,y,theta].