udacity 无迹卡尔曼滤波(UKF)python版本。根据网上的C++版本改写,方便大家参考。
参考的博客:
1无迹卡尔曼滤波(Unscented Kalman Filter, UKF):理论和应用_无迹卡尔曼滤波 python实现-CSDN博客
2无人驾驶技术——无损卡尔曼滤波(UKF)_c++ ukf-CSDN博客
感谢作者的无私分享。
一、引用的库
略。
二、定义矩阵类
略。
三、创建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)步骤。