def ekfStep(self, measurement):
self.calF()
self.predict()
self.x[2] = normalizeAngle(self.x[2])
self.update(measurement,
self.H_Jac,
self.H_state,
residual=self.residualWithAng)
def __init__(self):
self.x = [
10.,
]
self.y = [
0.,
]
#self.dt = 0.05
self.dt = 0.1
self.lVel = [0.0]
self.aVel = [
0.0,
]
self.vX = [-0.3]
self.vY = [0.]
self.refBearing = [
0.0,
]
self.bearing = [
0.0,
]
self.uwb = [np.linalg.norm([self.x[0], self.y[0]])]
self.uwbNoisy = [np.linalg.norm([self.x[0], self.y[0]])]
self.yaw = [
normalizeAngle(pi / 2 + self.aVel[0] * self.dt),
]
self.yawNoisy = [
normalizeAngle(pi / 2 + self.aVel[0] * self.dt),
]
self.timestamp = [0.]
self.lAcc = [0.]
self.dataSize = 10000
self.numDynamics = 5
self.targetLV = [0.0, 2.4, 1.7, 1.2, 3.1, 2.0, 2.7]
self.targetAV = [0.0, 1.5, -1.9, 2., 2.9, 1., 0.6]
self.dynamicTime = [0, 1000, 2000, 3000, 4000, 5000, 6000]
self.adaptionPeriod = 50
self.period = 0
self.dynamicStart = 0
self.doDynamics = False
self.notSetStep = True
self.notSetPeriod = True
def generate_sim(self):
for step in range(self.dataSize):
self.timestamp.append(step * self.dt)
if self.check_step_dymics(step):
interval = step - self.dynamicStart
x = pi * interval / self.adaptionPeriod
lv = self.targetLV[self.period - 1] + sin(
x / 2) * (self.targetLV[self.period] -
self.targetLV[self.period - 1])
av = sin(x) * self.targetAV[self.period]
else:
lv = self.lVel[step]
av = 0.
self.lAcc.append((lv - self.lVel[-1]) / self.dt)
self.lVel.append(lv)
self.aVel.append(av)
yaw_now = normalizeAngle(self.yaw[step] + av * self.dt)
yaw_noisy = yaw_now + np.random.normal(0, 0.01)
self.yaw.append(yaw_now)
self.yawNoisy.append(yaw_noisy)
self.vX.append(lv * cos(yaw_now))
self.vY.append(lv * sin(yaw_now))
del_displacement = lv * self.dt
x_now = self.x[step] + del_displacement * cos(yaw_now)
y_now = self.y[step] + del_displacement * sin(yaw_now)
self.x.append(x_now)
self.y.append(y_now)
ref_bearing = atan2(y_now, x_now)
self.refBearing.append(ref_bearing)
bearing = normalizeAngle(pi - yaw_now + ref_bearing)
self.bearing.append(bearing)
range_meas = np.linalg.norm([self.x[step], self.y[step]])
self.uwb.append(range_meas)
#range_noisy = range_meas + np.random.normal(0, 0.005)
range_noisy = range_meas + np.random.normal(0, 0.30)
self.uwbNoisy.append(range_noisy)
def residualWithAng(self, zmeas, zpre):
pi = math.pi
resVal = np.subtract(zmeas, zpre)
resVal[1] = normalizeAngle(resVal[1])
return resVal