Python UnscentedKalmanFilter.Q[2,2]方法代码示例

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import Q[2,2] [as 别名]
def test_circle():
    from filterpy.kalman import KalmanFilter
    from math import radians
    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]])

    std_noise = .1


    f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, kappa=0)
    f.x = np.array([50., 90., 0])
    f.P *= 100
    f.R = np.eye(2)*(std_noise**2)
    f.Q = np.eye(3)*.001
    f.Q[0,0]=0
    f.Q[2,2]=0

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

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

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


    kf.R = f.R
    kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
    kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)

    measurements = []
    results = []

    zs = []
    kfxs = []
    for t in range (0,12000):
        a = t / 30 + 90
        x = cos(radians(a)) * 50.+ randn() * std_noise
        y = sin(radians(a)) * 50. + randn() * std_noise
        # create measurement = t plus white noise
        z = np.array([x,y])
        zs.append(z)

        f.predict()
        f.update(z)

        kf.predict()
        kf.update(z)

        # save data
        results.append (hx(f.x))
        kfxs.append(kf.x)
        #print(f.x)

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

    print(results)
    if DO_PLOT:
        plt.plot(zs[:,0], zs[:,1], c='r', label='z')
        plt.plot(results[:,0], results[:,1], c='k', label='UKF')
        plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF')
        plt.legend(loc='best')
        plt.axis('equal')

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import Q[2,2] [as 别名]
def two_radar_constalt():
    dt = 0.05

    def hx(x):
        r1, b1 = hx.R1.reading_of((x[0], x[2]))
        r2, b2 = hx.R2.reading_of((x[0], x[2]))

        return array([r1, b1, r2, b2])
        pass

    def fx(x, dt):
        x_est = x.copy()
        x_est[0] += x[1] * dt
        return x_est

    vx = 100 / 1000  # meters/sec
    vz = 0.0

    f = UKF(dim_x=3, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
    aircraft = ACSim((0, 1), (vx * dt, vz * dt), 0.00)

    range_std = 1 / 1000.0
    bearing_std = 1 / 1000000.0

    R1 = RadarStation((0, 0), range_std, bearing_std)
    R2 = RadarStation((60, 0), range_std, bearing_std)

    hx.R1 = R1
    hx.R2 = R2

    f.x = array([aircraft.pos[0], vx, aircraft.pos[1]])
    f.R = np.diag([range_std ** 2, bearing_std ** 2, range_std ** 2, bearing_std ** 2])
    q = Q_discrete_white_noise(2, var=0.0002, dt=dt)
    # q = np.array([[0,0],[0,0.0002]])
    f.Q[0:2, 0:2] = q
    f.Q[2, 2] = 0.0002
    f.P = np.diag([0.1, 0.01, 0.1]) * 0.1

    track = []
    zs = []

    for i in range(int(500 / dt)):
        pos = aircraft.update()

        r1, b1 = R1.noisy_reading(pos)
        r2, b2 = R2.noisy_reading(pos)

        z = np.array([r1, b1, r2, b2])
        zs.append(z)
        track.append(pos.copy())

    zs = asarray(zs)

    xs, Ps = f.batch_filter(zs)
    ms, _, _ = f.rts_smoother(xs, Ps)

    track = asarray(track)
    time = np.arange(0, len(xs) * dt, dt)

    plt.figure()
    plt.subplot(311)
    plt.plot(time, track[:, 0])
    plt.plot(time, xs[:, 0])
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("x position (m)")

    plt.subplot(312)
    plt.plot(time, xs[:, 1] * 1000, label="UKF")
    plt.plot(time, ms[:, 1] * 1000, label="RTS")
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("velocity (m/s)")

    plt.subplot(313)
    plt.plot(time, xs[:, 2] * 1000, label="UKF")
    plt.plot(time, ms[:, 2] * 1000, label="RTS")
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("altitude (m)")
    plt.ylim([900, 1100])

    for z in zs[:10]:
        p = R1.z_to_x(z[0], z[1])
        # plt.scatter(p[0], p[1], marker='+', c='k')

        p = R2.z_to_x(z[2], z[3])
        # plt.scatter(p[0], p[1], marker='+', c='b')

    plt.show()