udacity 2维EKF Python根据网上的C++版本改写，方便大家参考。
用于处理激光数据。
一、引用的库
import random from math import * import matplotlib.pyplot as plt import decimal
二、定义矩阵类
来自网络资料。
class matrix:
def __init__(self, value):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
def zero(self, dimx, dimy):
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError("Invalid size of matrix")
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0 for row in range(dimy)] for col in range(dimx)]
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError("Invalid size of matrix")
else:
self.dimx = dim
self.dimy = dim
self.value = [[0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1
def show(self):
for i in range(self.dimx):
print(self.value[i])
print(' ')
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError("Matrices must be of equal dimensions to add")
else:
# add if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError("Matrices must be of equal dimensions to subtract")
else:
# subtract if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError("Matrices must be m*n and n*p to multiply")
else:
# multiply if correct dimensions
res = matrix([[]])
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
def transpose(self):
# compute transpose
res = matrix([[]])
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
def Cholesky(self, ztol=1.0e-5):
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i]) ** 2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError("Matrix not positive-definite")
res.value[i][i] = sqrt(d)
for j in range(i + 1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S) / res.value[i][i]
return res
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k] * res.value[j][k] for k in range(j + 1, self.dimx)])
res.value[j][j] = 1.0 / tjj ** 2 - S / tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = -sum(
[self.value[i][k] * res.value[k][j] for k in range(i + 1, self.dimx)]) / self.value[i][i]
return res
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
def __repr__(self):
return repr(self.value)
三、初始化变量
x = matrix([[0.], [0.], [0.], [0.]]) # initial state (location and velocity) P = matrix([[1., 0, 0, 0.],[0, 1, 0, 0],[0, 0, 1000, 0], [0, 0, 0., 1000.]]) # initial uncertainty u = matrix([[0.], [0.]]) # external motion F = matrix([[1., 0., 1., 0.], [0., 1., 0., 1.],[0., 0., 1., 0.],[0., 0., 0., 1.]]) # next state function H = matrix([[1., 0., 0, 0],[0, 1, 0, 0]]) # measurement function R = matrix([[0.0225, 0],[0, 0.0225]]) # measurement uncertainty Q = matrix([[0, 0, 0, 0],[0, 0, 0, 0],[0, 0, 0, 0],[0, 0, 0, 0]]) I = matrix([[1., 0., 0, 0], [0., 1., 0, 0],[0, 0, 1, 0],[0, 0, 0, 1]]) # identity matrix noise_ax = 5 noise_ay = 5
四、2D KF
def kalman_filter_2D(measurement):
global x
global P
# predict
x = F * x
P = F * P * F.transpose() + Q
# measurement update
Z = measurement
y = Z - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
return x, P
五、数据处理
注：以下程序用于处理激光数据。
time = 0.
b_init = 0
new_x = []
new_y = []
pre_t = 0.
dt = 0.
dt2 = 0.
dt3 = 0.
dt4 = 0.
meas = matrix([[0.], [0.]]) # initial measurement
file1 = open('obj_pose-laser-radar-synthetic-input.txt', 'r')
file2 = open('obj_pose-laser-radar-synthetic-output.txt', 'w+')
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.value[1][0] = float(line[2])
time = float(line[3])
if(b_init == 0):
b_init = 1
pre_t = time
x.value[0][0] = meas.value[0][0]
x.value[1][0] = meas.value[1][0]
file2.write(str(x.value[0][0]) + ' ' + str(x.value[1][0]) + ' ' + str(x.value[2][0]) + ' ' + str(x.value[3][0]) + '\n')
else:
dt = (time - pre_t) / 1000000.0
pre_t = time
dt2 = dt * dt
dt3 = dt2 * dt
dt4 = dt3 * dt
F.value[0][2] = dt
F.value[1][3] = dt
Q.value[0][0] = dt4 * noise_ax / 4
Q.value[0][2] = dt3 * noise_ax / 2
Q.value[1][1] = dt4 * noise_ay / 4
Q.value[1][3] = dt3 * noise_ay / 2
Q.value[2][0] = dt3 * noise_ax / 2
Q.value[2][2] = dt2 * noise_ax
Q.value[3][1] = dt3 * noise_ay / 2
Q.value[3][3] = dt2 * noise_ay
kalman_filter_2D(meas)
file2.write(str(x.value[0][0]) + ' ' + str(x.value[1][0]) + ' ' + str(x.value[2][0]) + ' ' + str(x.value[3][0]) + '\n')
file1.close()
file2.close()
六、图形显示
import decimal
import matplotlib.pyplot as plt
m_px = [] #measurement position
m_py = []
gt_px = [] #groudtruth position
gt_py = []
gt_vx = [] #groundtruth velocity
gt_vy = []
e_px = [] #estimation position
e_py = []
e_vx = [] #estimation velocity
e_vy = []
with open('obj_pose-laser-radar-synthetic-input.txt', 'r') as file:
data = file.readlines()
for line in data:
line = line.split('\t')
sensor = line[0]
if(sensor == 'L'):
m_px.append(float(line[1]))
m_py.append(float(line[2]))
gt_px.append(float(line[4]))
gt_py.append(float(line[5]))
gt_vx.append(float(line[6]))
gt_vy.append(float(line[7]))
with open('obj_pose-laser-radar-synthetic-output.txt', 'r') as file:
data = file.readlines()
for line in data:
line = line.split(' ')
e_px.append(float(line[0]))
e_py.append(float(line[1]))
e_vx.append(float(line[2]))
e_vy.append(float(line[3]))
'''
plt.scatter(m_px, m_py, c='red',linewidth = 1, s = 5)
plt.scatter(e_px,e_py,c='green',linewidth = 1, s = 5)
plt.scatter(gt_px,gt_py,c='blue',linewidth = 1, s = 5)
'''
plt.scatter(e_vx,e_vy,c='blue')
plt.scatter(gt_vx,gt_vy,c='yellow',linewidth = 1, s = 5)
plt.show()
print("end")
七、对比
Python版本代码运行结果与网上的C++版本运行结果一致。
八、关于速度
实际上，状态变量由轨迹和速度组成，上述代码能够得到速度预测值。绿色点表示目标移动轨迹，蓝色值表示目标移动速度。
下图更明显，黄色为目标速度真值，蓝色为目标速度预测值。
九、关于dt
本次数据中，发现激光雷达测量的时间间隔dt不变，为0.1；
十、关于Q
如果不考虑Q，预测轨迹如下图示，红色为观测轨迹，绿色为预测轨迹，差距明显。需要考虑Q。
十一、关于noise_ax,noise_ay
如何取值？
noise_ax = 3 noise_ay = 3 与 noise_ax = 5 noise_ay = 5 结果相差不大，从预测轨迹上看不出差距。
即使取1000，似乎偏差也不大。
十二、关于R
R的取值对轨迹预测影响挺大。
十三、目标位置&速度的真值、测量值、预测值对比
只采用激光传感器对目标位置以及速度进行预测，如下图：
（1）x方向预测对比
（2）y方向预测对比
（3）目标位置轨迹以及速度分布对比
（4）结论：
采用激光雷达对于速度的预测似乎也可以。
十四、计算RMSE
代码如下：
def calculate_rmse(est, truth):
rmse = matrix([[0., 0., 0., 0.]])
residual = matrix([[0., 0., 0., 0.]])
if est.dimx != truth.dimx:
print("error")
return rmse
for i in range(est.dimx):
for j in range(est.dimy):
residual.value[0][j] = est.value[i][j] - truth.value[i][j]
residual.value[0][j] = residual.value[0][j] * residual.value[0][j]
rmse.value[0][j] = rmse.value[0][j] + residual.value[0][j]
#rmse2.value[i][j] = math.sqrt(residual.value[0][j])
rmse = matrix([[math.sqrt(rmse.value[0][0] / est.dimx), math.sqrt(rmse.value[0][1] / est.dimx),
math.sqrt(rmse.value[0][2] / est.dimx), math.sqrt(rmse.value[0][3] / est.dimx)]])
return rmse
十五、Lidar测量值、预测值与真值对比
(1)测量值与真值的RMSE
备注：这里测量值只包含位置的测量值，没有速度的测量值；导致速度的RMSE较大。
(2)预测值与真值的RMSE
备注：预测值能够得到速度的预测值
(3)小结
采用卡尔曼滤波后降低了RMSE
十六、各种RMSE对比
(1)RMSE:X方向轨迹预测值与真值比较
(2)RMSE:Y方向轨迹预测值与真值比较
(3)RMSE:X方向速度预测值与真值比较
(4)RMSE:Y方向速度预测值与真值比较
(5)RMSE:所有RMSE比较
扫一扫
1452
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
