udacity 无迹卡尔曼滤波（UKF）-python版本(2）

udacity 无迹卡尔曼滤波（UKF）python版本。根据网上的C++版本改写，方便大家参考。

参考的文章：

（1）https://www.zhihu.com/people/lin-ming-33-56/posts?page=5

感谢作者的分享文章及代码。

一、引用的库

略。

二、定义矩阵类

略。

三、UKF滤波

（1）处理Radar数据

def Ukf(measurement, dt, x, p):
    n_x = 5
    n_aug = 7
    lambda1 = 3 - n_aug
    std_a = 1.6
    std_yawdd = 0.6

    Xsig_aug = matrix([[0] * 15 for _ in range(7)])
    x_aug = matrix([[0] * 1 for _ in range(7)])
    p_aug = matrix([[0] * 7 for _ in range(7)])

    for row in range(n_x):
        x_aug.value[row] = x.value[row]

    for col in range(n_x):
        for row in range(n_x):
            p_aug.value[row][col] = p.value[row][col]

    p_aug.value[5][5] = std_a * std_a
    p_aug.value[6][6] = std_yawdd * std_yawdd

    A = matrix([[0] * 7 for _ in range(7)])
    A = p_aug.Cholesky()
    A = A.transpose()

    for row in range(n_aug):
        Xsig_aug.value[row][0] = x_aug.value[row][0]

    for col in range(n_aug):
        for row in range(n_aug):
            Xsig_aug.value[row][col + 1] = x_aug.value[row][0] + sqrt(3) * A.value[row][col]
            Xsig_aug.value[row][col + 1 + n_aug] = x_aug.value[row][0] - sqrt(3) * A.value[row][col]

    #SigmaPointPrediction()
    Xsig_pred = matrix([[0] * 15 for _ in range(5)])

    delta_t = dt

    for col in range(15):
        p_x = Xsig_aug.value[0][col]
        p_y = Xsig_aug.value[1][col]
        v = Xsig_aug.value[2][col]
        yaw = Xsig_aug.value[3][col]
        yawd = Xsig_aug.value[4][col]
        v_a = Xsig_aug.value[5][col]
        v_b = Xsig_aug.value[6][col]

        if (fabs(yawd) > 0.001):
            px_p = p_x + v / yawd * (sin(yaw + yawd * delta_t) - sin(yaw))
            py_p = p_y + v / yawd * (cos(yaw) - cos(yaw + yawd * delta_t))
        else:
            px_p = p_x + v * delta_t * cos(yaw)
            py_p = p_y + v * delta_t * sin(yaw)

        v_p = v
        yaw_p = yaw + yawd * delta_t
        yawd_p = yawd

        px_p = px_p + 0.5 * v_a * delta_t * delta_t * cos(yaw)
        py_p = py_p + 0.5 * v_a * delta_t * delta_t * sin(yaw)
        v_p = v_p + v_a * delta_t
        yaw_p = yaw_p + 0.5 * v_b * delta_t * delta_t
        yawd_p = yawd_p + v_b * delta_t

        Xsig_pred.value[0][col] = px_p
        Xsig_pred.value[1][col] = py_p
        Xsig_pred.value[2][col] = v_p
        Xsig_pred.value[3][col] = yaw_p
        Xsig_pred.value[4][col] = yawd_p

    #PredictMeanAndCovariance()
    weights = matrix([[0] * 15 for _ in range(1)])
    x = matrix([[0] * 1 for _ in range(5)])
    p = matrix([[0] * 5 for _ in range(5)])
    x_diff = matrix([[0] * 1 for _ in range(5)])
    x_diff_w = matrix([[0] * 1 for _ in range(5)])

    weights.value[0][0] = lambda1 / (lambda1 + n_aug)
    weight = 0.5 / (lambda1 + n_aug)
    for col in range(1, 15):
        weights.value[0][col] = weight

    for col in range(15):
        for row in range(5):
            x.value[row][0] = x.value[row][0] + weights.value[0][col] * Xsig_pred.value[row][col]

    M_PI = 3.1415926
    for col in range(15):
        for row in range(5):
            x_diff.value[row][0] = Xsig_pred.value[row][col] - x.value[row][0]

            while (row == 3 and x_diff.value[row][0] > M_PI):
                x_diff.value[row][0] = x_diff.value[row][0] - 2 * M_PI
            while (row == 3 and x_diff.value[row][0] < -M_PI):
                x_diff.value[row][0] = x_diff.value[row][0] + 2 * M_PI

            x_diff_w.value[row][0] = weights.value[0][col] * x_diff.value[row][0]

        p = p + x_diff_w.__mul__(x_diff.transpose())

    #PredictRadarMeasurement():
    n_z = 3

    std_radr = 0.3
    std_radphi = 0.03
    std_radrd = 0.3

    z_diff = matrix([[0] * 1 for _ in range(3)])
    z_diff_w = matrix([[0] * 1 for _ in range(3)])

    Zsig = matrix([[0] * 15 for _ in range(3)])
    z_pred = matrix([[0] * 1 for _ in range(3)])
    S = matrix([[0] * 3 for _ in range(3)])
    R = matrix([[0] * 3 for _ in range(3)])

    for col in range(15):
        p_x = Xsig_pred.value[0][col]
        p_y = Xsig_pred.value[1][col]
        v = Xsig_pred.value[2][col]
        yaw = Xsig_pred.value[3][col]
        yawd = Xsig_pred.value[4][col]

        th_2 = sqrt(p_x * p_x + p_y * p_y)
        rho_z = th_2
        yaw_z = atan2(p_y, p_x)
        rhod_z = (p_x * cos(yaw) * v + p_y * sin(yaw) * v) / th_2

        Zsig.value[0][col] = rho_z
        Zsig.value[1][col] = yaw_z
        Zsig.value[2][col] = rhod_z

    for col in range(15):
        for row in range(3):
            z_pred.value[row][0] = z_pred.value[row][0] + weights.value[0][col] * Zsig.value[row][col]

    for col in range(15):
        for row in range(3):
            z_diff.value[row][0] = Zsig.value[row][col] - z_pred.value[row][0]

            while (row == 1 and z_diff.value[row][0] > M_PI):
                z_diff.value[row][0] = z_diff.value[row][0] - 2 * M_PI
            while (row == 1 and z_diff.value[row][0] < -M_PI):
                z_diff.value[row][0] = z_diff.value[row][0] + 2 * M_PI

            z_diff_w.value[row][0] = weights.value[0][col] * z_diff.value[row][0]

        S = S + z_diff_w.__mul__(z_diff.transpose())

    R = matrix([[std_radr * std_radr, 0, 0], [0, std_radphi * std_radphi, 0], [0, 0, std_radrd * std_radrd]])
    S = S + R

    #UpdateState():
    Tc = matrix([[0] * n_z for _ in range(n_x)])
    K = matrix([[3] * 1 for _ in range(5)])

    z = measurement

    for col in range(15):
        for row1 in range(5):
            x_diff.value[row1][0] = Xsig_pred.value[row1][col] - x.value[row1][0]

            while (row1 == 3 and x_diff.value[row1][0] > M_PI):
                x_diff.value[row1][0] = x_diff.value[row1][0] - 2 * M_PI
            while (row1 == 3 and x_diff.value[row1][0] < -M_PI):
                x_diff.value[row1][0] = x_diff.value[row1][0] + 2 * M_PI

            x_diff_w.value[row1][0] = weights.value[0][col] * x_diff.value[row1][0]

        for row2 in range(3):
            z_diff.value[row2][0] = Zsig.value[row2][col] - z_pred.value[row2][0]

            while (row2 == 1 and z_diff.value[row2][0] > M_PI):
                z_diff.value[row2][0] = z_diff.value[row2][0] - 2 * M_PI
            while (row2 == 1 and z_diff.value[row2][0] < -M_PI):
                z_diff.value[row2][0] = z_diff.value[row2][0] + 2 * M_PI

        Tc = Tc + x_diff_w.__mul__(z_diff.transpose())

    K = Tc.__mul__(S.inverse())

    z_diff = z - z_pred
    while (z_diff.value[1][0] > M_PI):
        z_diff.value[1][0] = z_diff.value[1][0] - 2 * M_PI
    while (z_diff.value[1][0] < -M_PI):
        z_diff.value[1][0] = z_diff.value[1][0] + 2 * M_PI

    x = x + K.__mul__(z_diff)
    KS = K.__mul__(S)
    p = p - KS.__mul__(K.transpose())
    return [x, p]

#end ukf()

（2）处理Lidar数据

    #PredictRadarMeasurement():
    n_z = 2

    std_laspx_ = 0.15
    std_laspy_ = 0.15

    z_diff = matrix([[0] * 1 for _ in range(2)])
    z_diff_w = matrix([[0] * 1 for _ in range(2)])

    Zsig = matrix([[0] * 15 for _ in range(2)])
    z_pred = matrix([[0] * 1 for _ in range(2)])
    S = matrix([[0] * 2 for _ in range(2)])
    R = matrix([[0] * 2 for _ in range(2)])

    for col in range(15):
        p_x = Xsig_pred.value[0][col]
        p_y = Xsig_pred.value[1][col]

        Zsig.value[0][col] = p_x
        Zsig.value[1][col] = p_y

    for col in range(15):
        for row in range(2):
            z_pred.value[row][0] = z_pred.value[row][0] + weights.value[0][col] * Zsig.value[row][col]

    for col in range(15):
        for row in range(2):
            z_diff.value[row][0] = Zsig.value[row][col] - z_pred.value[row][0]
            z_diff_w.value[row][0] = weights.value[0][col] * z_diff.value[row][0]

        S = S + z_diff_w.__mul__(z_diff.transpose())

    R = matrix([[std_laspx_ * std_laspx_, 0], [0, std_laspy_ * std_laspy_]])
    S = S + R

    #UpdateState():
    Tc = matrix([[0] * n_z for _ in range(n_x)])
    K = matrix([[2] * 1 for _ in range(5)])

    z = measurement

    for col in range(15):
        for row1 in range(5):
            x_diff.value[row1][0] = Xsig_pred.value[row1][col] - x.value[row1][0]
            x_diff_w.value[row1][0] = weights.value[0][col] * x_diff.value[row1][0]

        for row2 in range(2):
            z_diff.value[row2][0] = Zsig.value[row2][col] - z_pred.value[row2][0]

        Tc = Tc + x_diff_w.__mul__(z_diff.transpose())

    K = Tc.__mul__(S.inverse())

    z_diff = z - z_pred

    x = x + K.__mul__(z_diff)
    KS = K.__mul__(S)
    p = p - KS.__mul__(K.transpose())
    return [x, p]

#end ukf()

四、数据处理

（1）处理Radar数据

file1 = open('obj_pose-laser-radar-synthetic-input.txt', 'r')
file2 = open('obj_pose-laser-radar-synthetic-output-radar-ukf.txt', 'w+')

#input file format:
#L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
#R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
#Output file format:
#est_px est_py est_v gt_px gt_py

meas = matrix([[0] * 1 for _ in range(3)]) # initial measurement
x = matrix([[0] * 1 for _ in range(5)]) # state
p = matrix([[0] * 5 for _ in range(5)]) # covariance
b_init = 0

data = file1.readlines()
for line in data:
    line = line.split('\t')
    sensor = line[0]
    if(sensor == 'R'):
        meas.value[0][0] = float(line[1]) #rho
        meas.value[1][0] = float(line[2]) #phi
        meas.value[2][0] = float(line[3]) #rho-dot

        time = float(line[4])
        gt_px = float(line[5])
        gt_py = float(line[6])

        if(b_init == 0):
            b_init = 1
            pre_t = time

            px = meas.value[0][0] * cos(meas.value[1][0])
            py = meas.value[0][0] * sin(meas.value[1][0])
            vx = meas.value[2][0] * cos(meas.value[1][0])
            vy = meas.value[2][0] * sin(meas.value[1][0])

            x.value[0][0] = px
            x.value[1][0] = py
            x.value[2][0] = 0.
            x.value[3][0] = 0.
            x.value[4][0] = 0.

            p = matrix([[1, 0, 0, 0, 0],
                        [0, 1, 0, 0, 0],
                        [0, 0, 1, 0, 0],
                        [0, 0, 0, 1, 0],
                        [0, 0, 0, 0, 1]])

            file2.write(str(x.value[0][0]) + ' ' + #px
                        str(x.value[1][0]) + ' ' + #py
                        str(x.value[2][0]) + ' ' + #v
                        str(gt_px) + ' ' +         #gt_px
                        str(gt_py) +'\n')          #gt_py
        else:
            dt = (time - pre_t) / 1000000.0
            pre_t = time

            [x, p] = Ukf(meas, dt, x, p)
            file2.write(str(x.value[0][0]) + ' ' +  # px
                        str(x.value[1][0]) + ' ' +  # py
                        str(x.value[2][0]) + ' ' +  # v
                        str(gt_px) + ' ' +          # gt_px
                        str(gt_py) + '\n')          # gt_py
file1.close()
file2.close()

（2）处理Lidar数据

#main
file1 = open('obj_pose-laser-radar-synthetic-input.txt', 'r')
file2 = open('obj_pose-laser-radar-synthetic-output-lidar-ukf.txt', 'w+')

#input file format:
#L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
#R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
#Output file format:
#est_px est_py  gt_px gt_py

meas = matrix([[0] * 1 for _ in range(2)]) # initial measurement
x = matrix([[0] * 1 for _ in range(5)]) # state
p = matrix([[0] * 5 for _ in range(5)]) # covariance
b_init = 0

data = file1.readlines()
for line in data:
    line = line.split('\t')
    sensor = line[0]
    if(sensor == 'L'):
        meas.value[0][0] = float(line[1]) #meas_px
        meas.value[1][0] = float(line[2]) #meas_py

        time = float(line[3])
        gt_px = float(line[4])
        gt_py = float(line[5])

        if(b_init == 0):
            b_init = 1
            pre_t = time

            px = meas.value[0][0]
            py = meas.value[1][0]

            x.value[0][0] = px
            x.value[1][0] = py

            p = matrix([[1, 0, 0, 0, 0],
                        [0, 1, 0, 0, 0],
                        [0, 0, 1, 0, 0],
                        [0, 0, 0, 1, 0],
                        [0, 0, 0, 0, 1]])

            file2.write(str(x.value[0][0]) + ' ' + #px
                        str(x.value[1][0]) + ' ' + #py
                        str(gt_px) + ' ' +         #gt_px
                        str(gt_py) +'\n')          #gt_py
        else:
            dt = (time - pre_t) / 1000000.0
            pre_t = time

            [x, p] = Ukf(meas, dt, x, p)
            file2.write(str(x.value[0][0]) + ' ' +  # px
                        str(x.value[1][0]) + ' ' +  # py
                        str(gt_px) + ' ' +          # gt_px
                        str(gt_py) + '\n')          # gt_py
file1.close()
file2.close()

五、结果对比

（1）对于Radar数据，将ukf滤波获取的结果与真值以及ekf获取的结果进行对比，如下图示：

基本上ukf、ekf滤波结果差别不大。

（2）ukf滤波分别对Radar数据与Lidar数据处理，结果如下图示：

从图上看，同样是Ukf滤波，Lidar传感器的效果好于Radar传感器。

（3）Lidar数据，分别用Ukf滤波以及KF滤波进行处理，结果如下图示：

从图上看，对于Lidar传感器数据，采用ukf滤波与KF滤波效果差别不大。

六、关于cholesky分解

本程序中使用的cholesky分解函数对于某些矩阵会判定为非正定矩阵，报错；

本程序中尝试采用np.linalg.cholesky()进行分解，同样报错；

而c++程序中P.llt().matrixL()进行分解，却没有报错。

有知道原因的网友解释一下，谢谢。