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

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import Q[0:2,0:2] [as 别名]
def h_cv(x):
    return np.array([x[0], x[2]])

def e(x):
    res = []
    for n in range(x.shape[0]):
        res.append(np.sqrt(x[n][0]**2+x[n][2]**2))
    return res

dt = 1.0
random.seed(1234)

ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0)
ukf.x = np.array([100., 0., 0., 0.])
ukf.R = np.diag([25, 25])
ukf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ukf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04)

ckf = CubatureKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt)
ckf.x = np.array([100., 0., 0., 0.])
ckf.R = np.diag([25, 25])
ckf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ckf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04)

uxs = []
pxs = []
zs = []
txs = []
cxs = []
radius = 100
delta = 2*np.pi/360*5

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

    # x = [x, x', y, y']
    x = np.array([measurements[0][0], 0., measurements[0][1], 0.])

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

    # Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
    sigmas = MerweScaledSigmaPoints(n=4, alpha=0.3, beta=2., kappa=-3.)
    
    bot_filter = UKF(dim_x=4, dim_z=2, fx=f_linear, hx=h_linear, dt=dt, points=sigmas)
    bot_filter.x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
    #bot_filter.F = F
    bot_filter.H = np.asarray(H)
    #bot_filter.Q = Q
    bot_filter.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.1)
    bot_filter.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.1)
    bot_filter.P *= 10
    bot_filter.R = np.diag([0.0001, 0.0001])

    observable_meas = measurements[0:len(measurements)-60]

    pos, cov = [], []
    for z in observable_meas:
        pos.append(bot_filter.x)
        cov.append(bot_filter.P)
        
        bot_filter.predict()
        bot_filter.update(z)

    for i in range(0,60):
        bot_filter.predict()
        pos.append(bot_filter.x)
        
    return pos

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

    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
        x_est[2] += x[3] * dt
        return x_est

    f = UKF(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
    aircraft = ACSim((100, 100), (0.1 * dt, 0.02 * dt), 0.002)

    range_std = 0.2
    bearing_std = radians(0.5)

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

    hx.R1 = R1
    hx.R2 = R2

    f.x = array([100, 0.1, 100, 0.02])
    f.R = np.diag([range_std ** 2, bearing_std ** 2, range_std ** 2, bearing_std ** 2])
    q = Q_discrete_white_noise(2, var=0.002, dt=dt)
    # q = np.array([[0,0],[0,0.0002]])
    f.Q[0:2, 0:2] = q
    f.Q[2:4, 2:4] = q
    f.P = np.diag([0.1, 0.01, 0.1, 0.01])

    track = []
    zs = []

    for i in range(int(300 / 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, Pxz = f.batch_filter(zs)
    ms, _, _ = f.rts_smoother2(xs, Ps, Pxz)

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

    plt.figure()
    plt.subplot(411)
    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(412)
    plt.plot(time, track[:, 1])
    plt.plot(time, xs[:, 2])
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("y position (m)")

    plt.subplot(413)
    plt.plot(time, xs[:, 1])
    plt.plot(time, ms[:, 1])
    plt.legend(loc=4)
    plt.ylim([0, 0.2])
    plt.xlabel("time (sec)")
    plt.ylabel("x velocity (m/s)")

    plt.subplot(414)
    plt.plot(time, xs[:, 3])
    plt.plot(time, ms[:, 3])
    plt.ylabel("y velocity (m/s)")
    plt.legend(loc=4)
    plt.xlabel("time (sec)")

    plt.show()

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