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

原创已于 2024-06-15 20:31:31 修改 · 1k 阅读

14 ·

CC 4.0 BY-SA版权

文章标签：

#python #开发语言

于 2024-05-22 16:26:23 首次发布

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

参考的博客：
1无迹卡尔曼滤波（Unscented Kalman Filter, UKF）：理论和应用_无迹卡尔曼滤波 python实现-优快云博客

 2无人驾驶技术——无损卡尔曼滤波（UKF）_c++ ukf-优快云博客

感谢作者的无私分享。

一、引用的库

略。

二、定义矩阵类

略。

三、创建sigma点

使用当前状态估计和协方差矩阵生成sigma点。

def GenerateSigmaPoints():
    x = matrix([[5.7441], [1.3800], [2.2049], [0.5015], [0.3528]])

    p = matrix([[0.0043, -0.0013, 0.0030, -0.0022, -0.0020],
                [-0.0013, 0.0077, 0.0011, 0.0071, 0.0060],
                [0.0030, 0.0011, 0.0054, 0.0007, 0.0008],
                [-0.0022, 0.0071, 0.0007, 0.0098, 0.0100],
                [-0.0020, 0.0060, 0.0008, 0.0100, 0.0123]])

    n_x = 5
   
    lambda1 = 3 - n_x

    Xsig = matrix([[0] * 11 for _ in range(5)])

    A = matrix([[0] * 5 for _ in range(5)])
    A = p.Cholesky()
    A = A.transpose();
    #print(A)

    n_x = 5
    for row in range(n_x):
        Xsig.value[row][0] = x.value[row][0]

    for col in range(n_x):
        for row in range(n_x):
            Xsig.value[row][col + 1] = x.value[row][0] + sqrt(3) * A.value[row][col]
            Xsig.value[row][col + 1 + n_x] = x.value[row][0] - sqrt(3) * A.value[row][col]

    print(Xsig)

3.1 生成的sigma点值

3.2显示Sigma点

如下图示：

3.3 原始均值与方差

3.5生成sigma点的均值与方差

3.6结论

生成的sigma点保持了原始状态分布的均值与方差，代表了原始状态分布的均值与方差。

四、扩充sigma点

增加噪音状态量。

def AugmentedGenerateSigmaPoints():
    x = matrix([[5.7441],
                [1.3800],
                [2.2049],
                [0.5015],
                [0.3528]])

    p = matrix([[0.0043, -0.0013, 0.0030, -0.0022, -0.0020],
                [-0.0013, 0.0077, 0.0011, 0.0071, 0.0060],
                [0.0030, 0.0011, 0.0054, 0.0007, 0.0008],
                [-0.0022, 0.0071, 0.0007, 0.0098, 0.0100],
                [-0.0020, 0.0060, 0.0008, 0.0100, 0.0123]])

    n_x = 5
    n_aug = 7
    lambda1 = 3 - n_aug
    std_a = 0.2
    std_yawdd = 0.2

    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();
    # print(A)

    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]

    print(Xsig_aug)

4.1显示Sigma点

如下图示：

五、预测sigma点

对每个 sigma sigma点进行状态转移，得到预测状态

def SigmaPointPrediction():
    n_x = 5
    n_aug = 7

    Xsig_aug = matrix([[5.7441, 5.85768, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.63052, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441],
                       [1.38, 1.34566, 1.52806, 1.38, 1.38, 1.38, 1.38, 1.38, 1.41434, 1.23194, 1.38, 1.38, 1.38, 1.38, 1.38],
                       [2.2049, 2.28414, 2.24557, 2.29582, 2.2049, 2.2049, 2.2049, 2.2049, 2.12566, 2.16423, 2.11398, 2.2049, 2.2049, 2.2049, 2.2049],
                       [0.5015, 0.44339, 0.631886, 0.516923, 0.595227, 0.5015, 0.5015, 0.5015, 0.55961, 0.371114, 0.486077, 0.407773, 0.5015, 0.5015, 0.5015],
                       [0.3528, 0.299973, 0.462123, 0.376339, 0.48417, 0.418721, 0.3528, 0.3528, 0.405627, 0.243477, 0.329261, 0.22143, 0.286879, 0.3528, 0.3528],
                       [0, 0, 0, 0, 0, 0, 0.34641, 0, 0, 0, 0, 0, 0, -0.34641, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0.34641, 0, 0, 0, 0, 0, 0, -0.34641]])

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

    delta_t = 0.1

    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

    print(Xsig_pred)

5.1显示预测点

如下图所示：

六、预测mean以及covariance

根据预测状态计算均值和协方差。

def PredictMeanAndCovariance():
    n_x = 5
    n_aug = 7

    lambda1 = 3 - n_aug

    Xsig_pred = matrix([[5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744],
                       [1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486],
                       [2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049],
                       [0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048],
                        [0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159]])


    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())

    print(x)
    print(p)

七、预测测量值

def PredictRadarMeasurement():

    n_x = 5
    n_aug = 7
    n_z = 3

    lambda1 = 3 - n_aug

    std_radr = 0.3
    std_radphi = 0.0175
    std_radrd = 0.1

    weights = matrix([[0] * 15 for _ in range(1)])
    x = matrix([[0] * 1 for _ in range(5)])
    p = matrix([[0] * 5 for _ in range(5)])
    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)])

    Xsig_pred = matrix([[5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744],
                       [1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486],
                       [2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049],
                       [0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048],
                        [0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159]])


    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):
        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

    #print(Zsig)

    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]

    M_PI = 3.1415926
    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]

        #print(z_diff)
        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

    print(z_pred)
    print(S)

7.1显示预测的测量值

如下图示：

八、更新状态值

def UpdateState():
    n_x = 5
    n_aug = 7
    n_z = 3

    lambda1 = 3 - n_aug

    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)])
    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)])
    z = matrix([[0] * 1 for _ in range(3)])
    S = matrix([[0] * 3 for _ in range(3)])
    R = matrix([[0] * 3 for _ in range(3)])
    Tc = matrix([[0] * n_z for _ in range(n_x)])
    K = matrix([[3] * 1 for _ in range(5)])

    Xsig_pred = matrix([[5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744],
                       [1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486],
                       [2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049],
                       [0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048],
                        [0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159]])

    x = matrix([[5.93637],[1.49035],[2.20528],[0.536853],[0.353577]])

    p = matrix([[0.0054342, -0.002405, 0.0034157, -0.0034819, -0.00299378],
                [-0.002405, 0.01084, 0.001492, 0.0098018, 0.00791091],
                [0.0034157, 0.001492, 0.0058012, 0.00077863, 0.000792973],
                [-0.0034819, 0.0098018, 0.00077863, 0.011923, 0.0112491],
                [-0.0029937, 0.0079109, 0.00079297, 0.011249, 0.0126972]])

    Zsig = matrix([[6.1190, 6.2334, 6.1531, 6.1283, 6.1143, 6.1190, 6.1221, 6.1190, 6.0079, 6.0883, 6.1125, 6.1248, 6.1190, 6.1188, 6.12057],
                   [0.24428, 0.2337, 0.27316, 0.24616, 0.24846, 0.24428, 0.24530, 0.24428, 0.25700, 0.21692, 0.24433, 0.24193, 0.24428, 0.24515, 0.245239],
                   [2.1104, 2.2188, 2.0639, 2.187, 2.0341, 2.1061, 2.1450, 2.1092, 2.0016, 2.129, 2.0346, 2.1651, 2.1145, 2.0786, 2.11295]])

    z_pred = matrix([[6.12155],[0.245993],[2.10313]])

    S = matrix([[0.0946171, -0.000139448, 0.00407016],
                [-0.000139448, 0.000617548, -0.000770652],
                [0.00407016, -0.000770652, 0.0180917]])

    z = matrix([[5.9214],[0.2187],[2.0062]])

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

    M_PI = 3.1415926
    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())

    print(x)
    print(p)

九、功能调用

#GenerateSigmaPoints()
#AugmentedGenerateSigmaPoints()
#SigmaPointPrediction()
#PredictMeanAndCovariance()
#PredictRadarMeasurement()
UpdateState()

十、关于矩阵的逆

用到函数Cholesky，

def Cholesky(self, ztol=1.0e-5):

若ztol=1.0e-5，则结果与c++程序会有差异；

若ztol=1.0e-6，则结果与c++程序基本相同。

十一、问题

（1）初始的协方差矩阵P如何获取到？

udacity例子是提前给了P的值，实际自己做项目的时候，这个P如何设置？有知道的网友请赐教。

十二、关于状态预测UT变换以及量测预测UT变换

参考第05讲无迹卡尔曼滤波与联邦滤波_哔哩哔哩_bilibili

关于状态预测UT变换以及量测预测UT变换的介绍，得到如下图：

上图状态预测UT变换中，

（1）步骤相当于GenerateSigmaPoints（）

（2）步骤相当于SigmaPointPrediction（）

（3）、（4）步骤相当于PredictMeanAndCovariance（）

上图量测预测UT变换中，

（1）步骤相当于GenerateSigmaPoints()

(2)、（3）、（4）步骤相当于PredictRadarMeasurement（）

udacity教程中，省略了量测预测UT变换中的步骤（1），在完成状态预测UT变换的（1）、（2）、（3）、（4）步骤后，直接到了量测预测UT变换的（2）、（3）、（4）步骤。

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

1 条评论