As I am still a beginner when it comes to kalman filters, I was wondering if someone could provide me with help on how to solve my problem.
At the moment I am trying to estimate the position, velocity and acceleration of an e-puck robot. The robot comes equipped with encoders for the motors' position and an accelerometer giving a 3D acceleration vector in m/s^2.
Here is how I initialized my Unscented Kalman Filter:
points = sigPts(9, alpha=0.1, beta=2.0, kappa=0.0)
self._ukf = ukf(dim_x=9, dim_z=5, dt=TIME_STEP, hx=self.measurements, fx=EpuckNode.transition, points=points)
self._ukf.x = np.array([float(init_pos_x), 0.0, 0.0, float(init_pos_y), 0.0, 0.0, float(init_pos_z), 0.0, 0.0])
self._ukf.P = np.identity(9, dtype=float)
self._ukf.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=TIME_STEP, var=0.02)
self._ukf.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=TIME_STEP, var=0.02)
self._ukf.Q[6:9, 6:9] = Q_discrete_white_noise(3, dt=TIME_STEP, var=0.02)
self._ukf.R = np.diag([0.2 ** 2, 0.2 ** 2, 0.2 ** 2, 0.03 ** 2, 0.03 ** 2]) # Sensors' sensitivity
def transition(state, dt):
# Define the transition matrix
f = np.array([
[1, dt, (dt ** 2)/2, 0, 0, 0, 0, 0, 0],
[0, 1, dt, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, dt, (dt ** 2)/2, 0, 0, 0],
[0, 0, 0, 0, 1, dt, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, dt, (dt ** 2)/2],
[0, 0, 0, 0, 0, 0, 0, 1, dt],
[0, 0, 0, 0, 0, 0, 0, 0, 1]
], dtype=float)
# Return the array containing all the transforms
return np.dot(f, state)
def measurements(self, measures):
"""
Define the computation required for the measurements
:param measures: An array containing: [encoder_L, encoder_R, accel_X, accel_Y]
:return:
"""
# Compute the position and heading
left_encoder = measures[0]
right_encoder = measures[3]
# Compute the number of steps each wheel executed
left_steps_diff = left_encoder * MOT_STEP_DIST - self._leftStepsPrev # Expressed in meters.
right_steps_diff = right_encoder * MOT_STEP_DIST - self._rightStepsPrev # Expressed in meters.
# Compute the rotation and step differences
delta_theta = (right_steps_diff - left_steps_diff) / WHEEL_DISTANCE # Expressed in radiant.
delta_steps = (right_steps_diff + left_steps_diff) / 2 # Expressed in meters.
# Extract the robot's position and orientation
pos_x = self._pos_x + delta_steps*math.cos(self._pos_z + delta_theta/2) # Expressed in meters.
pos_y = self._pos_y + delta_steps*math.sin(self._pos_z + delta_theta/2) # Expressed in meters.
pos_z = self._pos_z + delta_theta # Expressed in radiant.
# Filter the measurements obtained from the accelerometer and the encoders
# Keep track of the number of steps for both wheels
self._leftStepsPrev = left_encoder * MOT_STEP_DIST # Expressed in meters.
self._rightStepsPrev = right_encoder * MOT_STEP_DIST # Expressed in meters.
# Return the array containing all the measurements
return np.array([pos_x, pos_y, pos_z, measures[2], measures[5]])
# Read the sensors' value
left_encoder = self.getLeftEncoder()
right_encoder = self.getRightEncoder()
accels = self._accelerometer.getValues()
# Predict the next state
self._ukf.predict()
self._ukf.update([left_encoder, right_encoder, accels[0], accels[1]])
# Update the robot's state
self._pos_x = self._ukf.x[0]
self._speed_x = self._ukf.x[1]
self._accel_x = self._ukf.x[2]
self._pos_y = self._ukf.x[3]
self._speed_y = self._ukf.x[4]
self._accel_y = self._ukf.x[5]
self._pos_z = self._ukf.x[6]
self._speed_z = self._ukf.x[7]
self._accel_z = self._ukf.x[8]
print(self._ukf.x)
Traceback (most recent call last):
self._ukf.update([left_encoder, right_encoder, accels[0], accels[1]])
File "/usr/local/lib/python2.7/dist-packages/filterpy/kalman/UKF.py", line 341, in update
y = self.residual_z(z, zp) #residual
ValueError: operands could not be broadcast together with shapes (4,) (5,)
Sincerely.