Python KalmanFilter.update方法代码示例

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
class KFMotionModel(object):
    def __init__(self,bb):
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
        self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

        self.kf.R[2:,2:] *= 10.
        self.kf.P[4:,4:] *= 1000.
        self.kf.P *= 10.
        self.kf.Q[-1,-1] *= 0.01
        self.kf.Q[4:,4:] *= 0.01

        self.kf.x[:4,0] = np.array(bb2z(bb))
        self.history = []
        self.predicted = False

    def update(self,bb):
        self.history = []
        bb = np.array(bb2z(bb))
        bb = np.expand_dims(bb, axis=1)
        self.kf.update(bb)
        self.predicted = False
        

    def predict(self):
        if not self.predicted:
            if((self.kf.x[6]+self.kf.x[2])<=0):
                self.kf.x[6] *= 0.0
            self.kf.predict()
            self.history.append(z2bb(self.kf.x))
            self.predicted=True
        return self.history[-1]

    def get_state(self):
        return z2bb(self.kf.x)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def kf_circle():
    from filterpy.kalman import KalmanFilter
    from math import radians
    import math
    def hx(x):
        radius = x[0]
        angle = x[1]
        x = cos(radians(angle)) * radius
        y = sin(radians(angle)) * radius
        return np.array([x, y])

    def fx(x, dt):
        return np.array([x[0], x[1]+x[2], x[2]])


    def hx_inv(x, y):
        angle = math.atan2(y,x)
        radius = math.sqrt(x*x + y*y)
        return np.array([radius, angle])


    std_noise = .1


    kf = KalmanFilter(dim_x=3, dim_z=2)
    kf.x = np.array([50., 0., 0.])

    F = np.array([[1., 0, 0.],
                  [0., 1., 1.,],
                  [0., 0., 1.,]])

    kf.F = F
    kf.P*= 100
    kf.H = np.array([[1,0,0],
                     [0,1,0]])

    kf.R = np.eye(2)*(std_noise**2)
    #kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)



    zs = []
    kfxs = []
    for t in range (0,2000):
        a = t / 30 + 90
        x = cos(radians(a)) * 50.+ randn() * std_noise
        y = sin(radians(a)) * 50. + randn() * std_noise

        z = hx_inv(x,y)
        zs.append(z)

        kf.predict()
        kf.update(z)

        # save data
        kfxs.append(kf.x)

    zs = np.asarray(zs)
    kfxs = np.asarray(kfxs)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_noisy_1d():
    f = KalmanFilter(dim_x=2, dim_z=1)

    f.x = np.array([[2.0], [0.0]])  # initial state (location and velocity)

    f.F = np.array([[1.0, 1.0], [0.0, 1.0]])  # state transition matrix

    f.H = np.array([[1.0, 0.0]])  # Measurement function
    f.P *= 1000.0  # covariance matrix
    f.R = 5  # state uncertainty
    f.Q = 0.0001  # process uncertainty

    fsq = SquareRootKalmanFilter(dim_x=2, dim_z=1)

    fsq.x = np.array([[2.0], [0.0]])  # initial state (location and velocity)

    fsq.F = np.array([[1.0, 1.0], [0.0, 1.0]])  # state transition matrix

    fsq.H = np.array([[1.0, 0.0]])  # Measurement function
    fsq.P = np.eye(2) * 1000.0  # covariance matrix
    fsq.R = 5  # state uncertainty
    fsq.Q = 0.0001  # process uncertainty

    measurements = []
    results = []

    zs = []
    for t in range(100):
        # create measurement = t plus white noise
        z = t + random.randn() * 20
        zs.append(z)

        # perform kalman filtering
        f.update(z)
        f.predict()

        fsq.update(z)
        fsq.predict()

        assert abs(f.x[0, 0] - fsq.x[0, 0]) < 1.0e-12
        assert abs(f.x[1, 0] - fsq.x[1, 0]) < 1.0e-12

        # save data
        results.append(f.x[0, 0])
        measurements.append(z)

    p = f.P - fsq.P
    print(f.P)
    print(fsq.P)

    for i in range(f.P.shape[0]):
        assert abs(f.P[i, i] - fsq.P[i, i]) < 0.01

    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2.0, 0]]).T
    f.P = np.eye(2) * 100.0
    m, c, _, _ = f.batch_filter(zs, update_first=False)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
class KalmanBoxTracker(object):
  """
  This class represents the internel state of individual tracked objects observed as bbox.
  """
  count = 0
  def __init__(self,bbox):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4)
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0

  def update(self,bbox):
    """
    Updates the state vector with observed bbox.
    """
    self.time_since_update = 0
    self.history = []
    self.hits += 1
    self.hit_streak += 1
    self.kf.update(convert_bbox_to_z(bbox))

  def predict(self):
    """
    Advances the state vector and returns the predicted bounding box estimate.
    """
    if((self.kf.x[6]+self.kf.x[2])<=0):
      self.kf.x[6] *= 0.0
    self.kf.predict()
    self.age += 1
    if(self.time_since_update>0):
      self.hit_streak = 0
    self.time_since_update += 1
    self.history.append(convert_x_to_bbox(self.kf.x))
    return self.history[-1]

  def get_state(self):
    """
    Returns the current bounding box estimate.
    """
    return convert_x_to_bbox(self.kf.x)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.):
    dt = 0.1

    kf2 = KalmanFilter(dim_x=2, dim_z=2)

    kf2.F = array ([[1., dt], [0., 1.]])
    kf2.H = array ([[1., 0.], [1., 0.]])
    kf2.x = array ([[0.], [0.]])
    kf2.Q = array ([[dt**3/3, dt**2/2],
                    [dt**2/2, dt]]) * 0.02
    kf2.P *= 100
    kf2.R[0,0] = wheel_sigma**2
    kf2.R[1,1] = gps_sigma**2


    random.seed(SEED)
    xs = []
    zs = []
    nom = []
    for i in range(1, 100):
        m0 = i + randn()*wheel_sigma
        m1 = i + randn()*gps_sigma
        if gps_sigma >1e40:
            m1 = -1e40

        z = array([[m0], [m1]])

        kf2.predict()
        kf2.update(z)

        xs.append(kf2.x.T[0])
        zs.append(z.T[0])
        nom.append(i)

    xs = asarray(xs)
    zs = asarray(zs)
    nom = asarray(nom)


    res = nom-xs[:,0]
    std_dev = np.std(res)
    print('fusion std: {:.3f}'.format (np.std(res)))

    if DO_PLOT:

        plt.subplot(211)
        plt.plot(xs[:,0])
        #plt.plot(zs[:,0])
        #plt.plot(zs[:,1])

        plt.subplot(212)
        plt.axhline(0)
        plt.plot(res)
        plt.show()

    print(kf2.Q)
    print(kf2.K)
    return std_dev

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_1d_0P():
    global inf
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    f.F = (np.array([[1., 1.],
                     [0., 1.]]))    # state transition matrix

    f.H = np.array([[1., 0.]])    # Measurement function
    f.R = np.array([[5.]])                 # state uncertainty
    f.Q = np.eye(2)*0.0001                 # process uncertainty
    f.P = np.diag([20., 20.])

    inf.x = f.x.copy()
    inf.F = f.F.copy()
    inf.H = np.array([[1.,0.]])    # Measurement function
    inf.R_inv *= 1./5                 # state uncertainty
    inf.Q = np.eye(2)*0.0001
    inf.P_inv = 0.000000000000000000001
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (50):
        # create measurement = t plus white noise
        z = t + random.randn()* np.sqrt(5)
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        try:
            print(t, inf.P)
        except:
            pass

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)

    #assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_against_kf():
    inv = np.linalg.inv

    dt = 1.0
    IM = np.eye(2)
    Q = np.array([[.25, 0.5], [0.5, 1]])

    F = np.array([[1, dt], [0, 1]])
    #QI = inv(Q)
    P = inv(IM)


    from filterpy.kalman import InformationFilter
    from filterpy.common import Q_discrete_white_noise

    #f = IF2(2, 1)
    r_std = .2
    R = np.array([[r_std*r_std]])
    RI = inv(R)

    '''f.F = F.copy()
    f.H = np.array([[1, 0.]])
    f.RI = RI.copy()
    f.Q = Q.copy()
    f.IM = IM.copy()'''


    kf = KalmanFilter(2, 1)
    kf.F = F.copy()
    kf.H = np.array([[1, 0.]])
    kf.R = R.copy()
    kf.Q = Q.copy()

    f0 = InformationFilter(2, 1)
    f0.F = F.copy()
    f0.H = np.array([[1, 0.]])
    f0.R_inv = RI.copy()
    f0.Q = Q.copy()



    #f.IM = np.zeros((2,2))


    for i in range(1, 50):
        z = i + (np.random.rand() * r_std)
        f0.predict()
        #f.predict()
        kf.predict()


        f0.update(z)
        #f.update(z)
        kf.update(z)

        print(f0.x.T, kf.x.T)
        assert np.allclose(f0.x, kf.x)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
class CarTracker:
    def __init__(self, dt, ID, position_x, position_y):

        self.p_x = KalmanFilter(dim_x=3, dim_z=1)
        self.p_y = KalmanFilter(dim_x=3, dim_z=1)
        self.p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
        self.p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])

        self.p_x.H = np.array([[1, 0., 0.]])
        self.p_y.H = np.array([[1, 0., 0.]])


        self.R_x_std = 0.01  # update to the correct value
        self.Q_x_std = 7      # update to the correct value
        self.R_y_std = 0.01  # update to the correct value
        self.Q_y_std = 7      # update to the correct value

        self.p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_y_std ** 2)
        self.p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_x_std ** 2)

        self.p_x.R *= self.R_x_std ** 2
        self.dt = dt
        self.ID = ID
        self.p_x.x = np.array([[position_x], [0.], [0.]])
        self.p_y.x = np.array([[position_y], [0.], [0.]])
        self.p_x.P *= 100.  # can very likely be set to 100.
        self.p_y.P *= 100.  # can very likely be set to 100.

        self.time_since_last_update = 0.0

        self.p_y.R *= self.R_y_std ** 2

    def update_pose(self,position_x, position_y):
        self.time_since_last_update = 0.0 # reset time since last update
        self.p_x.update([[position_x]])
        self.p_y.update([[position_y]])

    def predict_pose(self):
        self.time_since_last_update += self.dt
        self.p_x.predict()
        self.p_y.predict()

    def get_last_update_time(self):
        return self.time_since_last_update

    def get_position(self):
        return [self.p_x.x[0], self.p_y.x[0]]

    def get_current_error(self):
        return [(self.p_x.P[0])[0], (self.p_y.P[0])[0]]

    def get_total_speed(self):
        # calculate magnitude of speed
        return np.sqrt(self.p_x.x[1]**2+self.p_y.x[1]**2)

    def get_ID(self):
        return self.ID

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_noisy_1d():
    f = KalmanFilter(dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    f.F = np.array([[1., 1.],
                    [0., 1.]])    # state transition matrix

    f.H = np.array([[1., 0.]])    # Measurement function
    f.P *= 1000.                  # covariance matrix
    f.R = 5                       # state uncertainty
    f.Q = 0.0001                  # process uncertainty

    measurements = []
    results = []

    zs = []
    for t in range(100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.update(z)
        f.predict()

        # save data
        results.append(f.x[0, 0])
        measurements.append(z)

        # test mahalanobis
        a = np.zeros(f.y.shape)
        maha = scipy_mahalanobis(a, f.y, f.SI)
        assert f.mahalanobis == approx(maha)


    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2., 0]]).T
    f.P = np.eye(2) * 100.
    s = Saver(f)
    m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
    s.to_array()
    assert len(s.x) == len(zs)
    assert len(s.x) == len(s)

    # plot data
    if DO_PLOT:
        p1, = plt.plot(measurements, 'r', alpha=0.5)
        p2, = plt.plot(results, 'b')
        p4, = plt.plot(m[:, 0], 'm')
        p3, = plt.plot([0, 100], [0, 100], 'g')  # perfect result
        plt.legend([p1, p2, p3, p4],
                   ["noisy measurement", "KF output", "ideal", "batch"], loc=4)
        plt.show()

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_1d():
    global inf
    f = KalmanFilter(dim_x=2, dim_z=1)
    inf = InformationFilter(dim_x=2, dim_z=1)

    # ensure __repr__ doesn't assert
    str(inf)


    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    inf.x = f.x.copy()
    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    inf.F = f.F.copy()
    f.H = np.array([[1.,0.]])      # Measurement function
    inf.H = np.array([[1.,0.]])    # Measurement function
    f.R *= 5                       # state uncertainty
    inf.R_inv *= 1./5               # state uncertainty
    f.Q *= 0.0001                  # process uncertainty
    inf.Q *= 0.0001


    m = []
    r = []
    r2 = []
    zs = []
    s = Saver(inf)
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.update(z)
        f.predict()

        inf.update(z)
        inf.predict()

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)
        print(inf.y)
        s.save()

        assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_1d_0P():
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    f.H = np.array([[1.,0.]])    # Measurement function
    f.R = np.array([[5.]])                 # state uncertainty
    f.Q = np.eye(2)*0.0001                 # process uncertainty
    f.P = np.diag([20., 20.])

    inf.x = f.x.copy()
    inf.F = f.F.copy()
    inf.H = np.array([[1.,0.]])    # Measurement function
    inf.R_inv *= 1./5                 # state uncertainty
    inf.Q *= 0.0001
    inf.P_inv = 0
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)

        #assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_univariate():
    f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
    f.x = np.array([[0]])
    f.P *= 50
    f.H = np.array([[1.]])
    f.F = np.array([[1.]])
    f.B = np.array([[1.]])
    f.Q = .02
    f.R *= .1

    for i in range(50):
        f.predict()
        f.update(i)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def single_measurement_test():
    dt = 0.1
    sigma = 2.

    kf2 = KalmanFilter(dim_x=2, dim_z=1)

    kf2.F = array ([[1., dt], [0., 1.]])
    kf2.H = array ([[1., 0.]])
    kf2.x = array ([[0.], [1.]])
    kf2.Q = array ([[dt**3/3, dt**2/2],
                    [dt**2/2, dt]]) * 0.02
    kf2.P *= 100
    kf2.R[0,0] = sigma**2

    random.seed(SEED)
    xs = []
    zs = []
    nom = []
    for i in range(1, 100):
        m0 = i + randn()*sigma
        z = array([[m0]])

        kf2.predict()
        kf2.update(z)

        xs.append(kf2.x.T[0])
        zs.append(z.T[0])
        nom.append(i)

    xs = asarray(xs)
    zs = asarray(zs)
    nom = asarray(nom)


    res = nom-xs[:,0]
    std_dev = np.std(res)
    print('std: {:.3f}'.format (std_dev))

    global DO_PLOT
    if DO_PLOT:

        plt.subplot(211)
        plt.plot(xs[:,0])
        #plt.plot(zs[:,0])


        plt.subplot(212)
        plt.plot(res)
        plt.show()

    return std_dev

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_procedure_form():

    dt = 1.
    std_z = 10.1

    x = np.array([[0.], [0.]])
    F = np.array([[1., dt], [0., 1.]])
    H = np.array([[1.,0.]])
    P = np.eye(2)
    R = np.eye(1)*std_z**2
    Q = Q_discrete_white_noise(2, dt, 5.1)

    kf = KalmanFilter(2, 1)
    kf.x = x.copy()
    kf.F = F.copy()
    kf.H = H.copy()
    kf.P = P.copy()
    kf.R = R.copy()
    kf.Q = Q.copy()


    measurements = []
    results = []

    xest = []
    ks = []
    pos = 0.
    for t in range (2000):
        z = pos + random.randn() * std_z
        pos += 100

        # perform kalman filtering
        x, P = predict(x, P, F, Q)
        kf.predict()
        assert norm(x - kf.x) < 1.e-12
        x, P, _, _, _, _ = update(x, P, z, R, H, True)
        kf.update(z)
        assert norm(x - kf.x) < 1.e-12

        # save data
        xest.append (x.copy())
        measurements.append(z)

    xest = np.asarray(xest)
    measurements = np.asarray(measurements)
    # plot data
    if DO_PLOT:
        plt.plot(xest[:, 0])
        plt.plot(xest[:, 1])
        plt.plot(measurements)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import update [as 别名]
def test_noisy_11d():
    f = KalmanFilter(dim_x=2, dim_z=1)

    f.x = np.array([2.0, 0])  # initial state (location and velocity)

    f.F = np.array([[1.0, 1.0], [0.0, 1.0]])  # state transition matrix

    f.H = np.array([[1.0, 0.0]])  # Measurement function
    f.P *= 1000.0  # covariance matrix
    f.R = 5  # state uncertainty
    f.Q = 0.0001  # process uncertainty

    measurements = []
    results = []

    zs = []
    for t in range(100):
        # create measurement = t plus white noise
        z = t + random.randn() * 20
        zs.append(z)

        # perform kalman filtering
        f.update(z)
        f.predict()

        # save data
        results.append(f.x[0])
        measurements.append(z)

    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2.0, 0]]).T
    f.P = np.eye(2) * 100.0
    m, c, _, _ = f.batch_filter(zs, update_first=False)

    # plot data
    if DO_PLOT:
        p1, = plt.plot(measurements, "r", alpha=0.5)
        p2, = plt.plot(results, "b")
        p4, = plt.plot(m[:, 0], "m")
        p3, = plt.plot([0, 100], [0, 100], "g")  # perfect result
        plt.legend([p1, p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4)

        plt.show()