def _init_chassis_kf(self, initial_meas):
'''
Initializes self._chassis_kf to 0 position and acceleration and initial
velocity based on the initial measurements.
@params initial_meas - (2, 1) np.array world frame relative [[vx], [vy]]
'''
self._chassis_kf = KalmanFilter(dynamParams=self._chassis_D,
measureParams=self._chassis_M)
# transitionMatrix will be edited before each predict/correct step with coefficients for the delta_t
self._chassis_kf.transitionMatrix = np.eye(
self._chassis_D,
dtype=np.float32) # transitionMatrix @ state_n = state_n+1
self._chassis_kf.measurementMatrix = np.array(
[ # predictedMeasurements += measurementMatrix @ statePre_n
[0, 0, 1, 0, 0, 0], # vx
[0, 0, 0, 1, 0, 0] # vy
],
np.float32)
# guessed covariances.
self._chassis_kf.processNoiseCov = np.diag(
np.array([0.05**2, 0.05**2, 0.1**2, 0.1**2, 0.01**2, 0.01**2],
dtype=np.float32))
self._chassis_kf.measurementNoiseCov = 0.005 * np.eye(self._chassis_M,
dtype=np.float32)
# initial state
# opencv KalmanFilter initializes both statePre and statePost to 0 proper vectors
vel_start = 2
vel_end = vel_start + self._chassis_M
self._chassis_kf.statePre[vel_start:vel_end,
0] = initial_meas[:self._chassis_M, 0]
self._chassis_kf.statePost[vel_start:vel_end,
0] = initial_meas[:self._chassis_M, 0]
def general_hough_closure(reference_image):
'''
Generator function to create a closure with the reference image and origin
at the center of the reference image
Returns a function f, which takes a query image and returns the accumulator
'''
kf = KalmanFilter()
kf.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kf.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
referencePoint = (reference_image.shape[0] / 2, reference_image.shape[1] / 2)
r_table = build_r_table(reference_image, referencePoint)
def f(query_image):
return accumulate_gradients(r_table, query_image)
return f
def _init_imu_kf(self, initial_meas):
'''
Initializes self._imu_kf to 0 angular position and acceleration and initial
velocity based on the initial measurements.
@params initial_meas - (7, 1) np.array IMU frame relative
[[roll], [pitch], [yaw], [wx], [wy], [wz], [yaw based on chassis yaw]]
'''
self._imu_kf = KalmanFilter(dynamParams=self._imu_D,
measureParams=self._imu_M)
# transitionMatrix will be edited before each predict/correct step with coefficients for the delta_t
self._imu_kf.transitionMatrix = np.eye(
self._imu_D,
dtype=np.float32) # transitionMatrix @ state_n = state_n+1
self._imu_kf.measurementMatrix = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
],
dtype=np.float32)
# guessed covariances.
self._imu_kf.processNoiseCov = np.diag(
np.array([
0.05**2, 0.05**2, 0.05**2, 0.1**2, 0.1**2, 0.1**2, 0.01**2,
0.01**2, 0.01**2
],
dtype=np.float32))
self._imu_kf.measurementNoiseCov = np.diag(
np.array([
0.005**2, 0.005**2, 0.05**2, 0.01**2, 0.01**2, 0.01**2, 0.0075
**2
],
dtype=np.float32))
# initial state
self._imu_kf.statePre[:self._imu_M, 0] = initial_meas[:self._imu_M, 0]
self._imu_kf.statePost[:self._imu_M, 0] = initial_meas[:self._imu_M, 0]
def init_kalmanfilter(detection):
KF = KalmanFilter(4, 2)
dt = 1.0 / 30.0
KF.transitionMatrix = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]]).astype(np.float32)
KF.processNoiseCov = (np.diag([20, 20, 150, 150]).astype(np.float32) ** 2) * dt
KF.statePost = np.array([[detection[0]],
[detection[1]],
[0],
[0]]).astype(np.float32)
KF.errorCovPost = np.diag([10, 10, 200, 200]).astype(np.float32) ** 2
KF.measurementMatrix = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]]).astype(np.float32)
KF.measurementNoiseCov = (np.eye(2).astype(np.float32) * 10) ** 2
KF.statePre = KF.statePost # maybe necessary?
KF.errorCovPre = KF.errorCovPost
return KF
def __init__(self, position, velocity):
self.kf = KalmanFilter(2, 1, 0)
self.kf.statePost = np.array([[position], [velocity]])
self.kf.errorCovPost = 1. * np.ones((2, 2))
self.kf.transitionMatrix = np.array([[1., 1.], [0., 1.]])
self.kf.measurementMatrix = 1. * np.ones((1, 2))
self.kf.measurementMatrix[0, 1] = 0.
self.kf.processNoiseCov = 1e-5 * np.eye(2)
self.kf.measurementNoiseCov = 1e-3 * np.ones((1, 1))
# self.position = position
# self.velocity = velocity
# self.old_position = None
self.m = 1
self.n = 1
'''
import os
from collections import defaultdict
from cv2 import KalmanFilter
import matplotlib.pyplot as plt
import numpy as np
from scipy.misc import imread
from scipy.ndimage.filters import sobel
# Good for the b/w test images used
from skimage.feature import canny
MIN_CANNY_THRESHOLD = 10
MAX_CANNY_THRESHOLD = 50
kalman = KalmanFilter(4, 2)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
kalman.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) * 0.03
mp = np.array((2, 1), np.float32) # measurement
def gradient_orientation(image):
'''
Calculate the gradient orientation for edge point in the image
'''
dx = sobel(image, axis=0, mode='constant')
dy = sobel(image, axis=1, mode='constant')
gradient = np.arctan2(dy, dx) * 180 / np.pi
return gradient
#tests PID on whole system from PID import PIDControl from BBB.roboclaw import Roboclaw from cv2 import KalmanFilter #import vijay/oliver's ultrasonic function pidCoefficients = [1, 0, 0] #still need to figure these out maxSpeed = 100 #we need to test what the max rotational velocity of the wheels on the robot are pid = PIDControl(0,[1,0,0],maxSpeed) #encProt = EncoderProtractor(0, wheelCircum, robotDiam, encoderResolution) """INITIALIZE KALMAN FILTER""" #we might need to use numpy based matrices for kalman implementation kalman = KalmanFilter(nstateValues, nMeasurementValues, nControlInputs) #kalman.transition_matrix = .... #kalman.measurement_matrix = .... #kalman.control_matrix = .... """#################""" #########initiate ultrasonic object here####### claw = Roboclaw(0x80, "/dev/ttyO1") curM1 = 50 curM2 = 50 claw.m1_forward(curM1) #I dont know what a good speed is yet claw.m2_forward(curM2)
class SentinelDrive(DriveOdometry):
'''
Derived class of DriveOdometry.
Tracks the odometry of the sentinel robot
'''
def __init__(self):
super(SentinelDrive, self).__init__()
def _init_vars(self):
'''
Initialize state variables.
All angular variables are in radians.
All self._chassis_kf variables are relative to the static world frame ("odom")
All self._imu_kf variables are relative to the IMU on the Type-A MCB
'''
# 2 Kalman Filters
# self._chassis_kf is for chassis relative odometry
# self._imu_kf is for imu relative odometry
self._chassis_kf = None
# number of state params for self._chassis_kf
# 6-D state: x, y and first and second derivatives in that order
self._chassis_D = 6
# number of measurement params for self._chassis_kf
# 2-D measurement: vx, vy
self._chassis_M = 2
# no control vars for self._chassis_kf
# Wheel odom vars
self.GEAR_RATIO = 19.0
self.WHEEL_RADIUS = 0.035
self._imu_kf = None
# number of state params for self._imu_kf
# 9-D state: roll, pitch, yaw and first and second derivatives in that order
self._imu_D = 9
# number of measurement params for self._imu_kf
# 6-D measurement: rol, pit, yaw and first derivatives
self._imu_M = 6
# no control vars for self._imu_kf
def _publish_odom(self, timestamp):
'''
See base class comment
'''
# One of the kalman filters have not been initialized yet, don't publish
if not self._chassis_kf or not self._imu_kf:
return
if self._should_reset():
rospy.logwarn("Odometry diverged, resetting kalman filters...")
self._reset_kf()
return
odom_msg = Odometry()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "base_center" # fitting 2 frame transforms into 1 odom msg
odom_msg.header.stamp = timestamp
chassis_state = self._chassis_kf.statePost[:, 0]
imu_state = self._imu_kf.statePost[:, 0]
# Translation
(odom_msg.pose.pose.position.x,
odom_msg.pose.pose.position.y) = chassis_state[0:2]
odom_msg.pose.pose.position.z = 0
# Rotation
mod = 2 * math.pi
mcb_imu_quat = tf_conversions.transformations.quaternion_from_euler(
imu_state[0] % mod, imu_state[1] % mod, imu_state[2] % mod)
pose_quat = convert_quat_to_frame(mcb_imu_quat, "base_center",
"mcb_imu")
(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w) = pose_quat
# Set pose covariances
chassis_cov = self._chassis_kf.errorCovPost
imu_cov = self._imu_kf.errorCovPost
pose_cov_matrix = np.zeros((6, 6), dtype=np.float32)
pose_cov_matrix[0:2, 0:2] = chassis_cov[0:2, 0:2]
pose_cov_matrix[3:6, 3:6] = imu_cov[0:3, 0:3]
odom_msg.pose.covariance = tuple(pose_cov_matrix.flatten())
# Translational velocity
(odom_msg.twist.twist.linear.x,
odom_msg.twist.twist.linear.y) = chassis_state[2:4]
odom_msg.twist.twist.linear.z = 0
# Angular velocities
twist_euler = convert_angv_to_frame(imu_state[3:6], "base_center",
"mcb_imu")
(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y,
odom_msg.twist.twist.angular.z) = twist_euler[0:3]
# Set velocity covariances
twist_cov_matrix = np.zeros((6, 6), dtype=np.float32)
twist_cov_matrix[0:2, 0:2] = chassis_cov[2:4, 2:4]
twist_cov_matrix[3:6, 3:6] = imu_cov[3:6, 3:6]
odom_msg.twist.covariance = tuple(twist_cov_matrix.flatten())
self._odom_pub.publish(odom_msg)
def _publish_accel(self, timestamp):
'''
See base class comment
'''
# One of the kalman filters have not been initialized yet, don't publish
if not self._imu_kf or not self._chassis_kf:
return
accel_msg = AccelStamped()
accel_msg.header.frame_id = "odom"
accel_msg.header.stamp = timestamp
(accel_msg.accel.linear.x,
accel_msg.accel.linear.y) = self._chassis_kf.statePost[4:6, 0]
accel_msg.accel.linear.z = 0
(accel_msg.accel.angular.x, accel_msg.accel.angular.y,
accel_msg.accel.angular.z) = self._imu_kf.statePost[6:9, 0]
self._accel_pub.publish(accel_msg)
def _init_chassis_kf(self, initial_meas):
'''
Initializes self._chassis_kf to 0 position and acceleration and initial
velocity based on the initial measurements.
@params initial_meas - (2, 1) np.array world frame relative [[vx], [vy]]
'''
self._chassis_kf = KalmanFilter(dynamParams=self._chassis_D,
measureParams=self._chassis_M)
# transitionMatrix will be edited before each predict/correct step with coefficients for the delta_t
self._chassis_kf.transitionMatrix = np.eye(
self._chassis_D,
dtype=np.float32) # transitionMatrix @ state_n = state_n+1
self._chassis_kf.measurementMatrix = np.array(
[ # predictedMeasurements += measurementMatrix @ statePre_n
[0, 0, 1, 0, 0, 0], # vx
[0, 0, 0, 1, 0, 0] # vy
],
np.float32)
# guessed covariances.
self._chassis_kf.processNoiseCov = np.diag(
np.array([0.05**2, 0.05**2, 0.1**2, 0.1**2, 0.01**2, 0.01**2],
dtype=np.float32))
self._chassis_kf.measurementNoiseCov = 0.005 * np.eye(self._chassis_M,
dtype=np.float32)
# initial state
# opencv KalmanFilter initializes both statePre and statePost to 0 proper vectors
vel_start = 2
vel_end = vel_start + self._chassis_M
self._chassis_kf.statePre[vel_start:vel_end,
0] = initial_meas[:self._chassis_M, 0]
self._chassis_kf.statePost[vel_start:vel_end,
0] = initial_meas[:self._chassis_M, 0]
def _init_imu_kf(self, initial_meas):
'''
Initializes self._imu_kf to 0 angular position and acceleration and initial
velocity based on the initial measurements.
@params initial_meas - (6, 1) np.array IMU frame relative
[[roll], [pitch], [yaw], [wx], [wy], [wz]]
'''
self._imu_kf = KalmanFilter(dynamParams=self._imu_D,
measureParams=self._imu_M)
# transitionMatrix will be edited before each predict/correct step with coefficients for the delta_t
self._imu_kf.transitionMatrix = np.eye(
self._imu_D,
dtype=np.float32) # transitionMatrix @ state_n = state_n+1
self._imu_kf.measurementMatrix = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
],
dtype=np.float32)
# guessed covariances.
self._imu_kf.processNoiseCov = np.diag(
np.array([
0.05**2, 0.05**2, 0.05**2, 0.1**2, 0.1**2, 0.1**2, 0.01**2,
0.01**2, 0.01**2
],
dtype=np.float32))
self._imu_kf.measurementNoiseCov = np.diag(
np.array([0.005**2, 0.005**2, 0.01**2, 0.01**2, 0.01**2, 0.02**2],
dtype=np.float32))
# initial state
self._imu_kf.statePre[:self._imu_M, 0] = initial_meas[:self._imu_M, 0]
self._imu_kf.statePost[:self._imu_M, 0] = initial_meas[:self._imu_M, 0]
def _configure_chassis_kf_for_update(self, delta_t):
'''
Configures self._chassis_kf for the predict step based on the time
difference between last measurement and current measurement
as part of the extended Kalman Filter
@params delta_t. Time difference between last measurement and current measurement
'''
vel = delta_t # velocity/2nd-derivative factor
acc = 0.5 * delta_t**2
np.fill_diagonal(self._chassis_kf.transitionMatrix[:4, 2:], vel)
np.fill_diagonal(self._chassis_kf.transitionMatrix[:2, 4:], acc)
def _configure_imu_kf_for_update(self, delta_t):
'''
Configures self._imu_kf for the predict step based on the time
difference between last measurement and current measurement
as part of the extended Kalman Filter
@params delta_t. Time difference between last measurement and current measurement
'''
vel = delta_t # velocity/2nd-derivative factor
acc = 0.5 * delta_t**2
np.fill_diagonal(self._imu_kf.transitionMatrix[:6, 3:], vel)
np.fill_diagonal(self._imu_kf.transitionMatrix[:3, 6:], acc)
def update(self, imu_stamped):
'''
Updates the current odometry state of this object by
carrying out the predict and update step on self._chassis_kf
and self._imu_kf
@params imu_stamped - the McbOdomMessage ROS message that contains the raw
sensor data from the MCB
'''
current_timestamp = imu_stamped.header.stamp.to_sec()
# Must update IMU first and then chassis
# bec chassis uses yaw readings of the IMU to determine direction of motion
# IMU update
current_imu_meas = np.array([
[imu_stamped.rol],
[imu_stamped.pit],
[imu_stamped.yaw],
[imu_stamped.wx],
[imu_stamped.wy],
[imu_stamped.wz],
], np.float32)
if not self._imu_kf:
self._init_imu_kf(current_imu_meas)
self._configure_imu_kf_for_update(
current_timestamp - self._last_update_timestamp.to_sec() if self.
_last_update_timestamp else 0)
self._imu_kf.predict()
self._imu_kf.correct(current_imu_meas)
# Chassis update
# Convert from RPM to radians/s
rf = -(imu_stamped.rf_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
lf = (imu_stamped.lf_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
lb = (imu_stamped.lb_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
rb = -(imu_stamped.rb_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
ave_mag = (rf + lf + lb + rb) / 4
signed_mag = self.WHEEL_RADIUS * ave_mag
# Convert yaw from IMU frame to chassis frame
yaw = convert_euler_to_frame(self._imu_kf.statePost[0:3, 0],
"base_center", "mcb_imu")[2]
vx = -signed_mag * math.sin(yaw)
vy = signed_mag * math.cos(yaw)
current_chassis_meas = np.array([[vx], [vy]], np.float32)
if not self._chassis_kf:
self._init_chassis_kf(current_chassis_meas)
self._configure_chassis_kf_for_update(
current_timestamp - self._last_update_timestamp.to_sec() if self.
_last_update_timestamp else 0)
self._chassis_kf.predict()
self._chassis_kf.correct(current_chassis_meas)
self._last_update_timestamp = imu_stamped.header.stamp
def _should_reset(self):
'''
See base class comment
'''
if self._imu_kf and self._chassis_kf:
abs_imu_state = np.abs(self._imu_kf.statePost)
abs_chassis_state = np.abs(self._chassis_kf.statePost)
abs_imu_error = np.abs(self._imu_kf.errorCovPost)
abs_chassis_error = np.abs(self._chassis_kf.errorCovPost)
return (np.any(np.isnan(abs_imu_error))
or np.any(np.isnan(abs_chassis_error))
or np.any(abs_imu_state[3:, :] > 100)
or np.any(abs_chassis_state[2:, :] > 100))
return False
def _reset_kf(self):
'''
Clears the 2 Kalman Filters' states
'''
self._chassis_kf = None
self._imu_kf = None
class FourWheelMecanumDrive(DriveOdometry):
'''
Derived class of DriveOdometry.
Tracks the odometry of robots with four wheel mecanum drives
'''
def __init__(self):
super(FourWheelMecanumDrive, self).__init__()
def _init_vars(self):
'''
Initialize state variables.
All angular variables are in radians.
All self._chassis_kf variables are relative to the static world frame ("odom")
All self._imu_kf variables are relative to the IMU on the Type-A MCB
'''
# 2 Kalman Filters
# self._chassis_kf is for chassis relative odometry
# self._imu_kf is for imu relative odometry
self._chassis_kf = None
# number of state params for self._chassis_kf
# 12-D state: x, y, z, chassis-relative yaw and their first and second derivatives in that order
self._chassis_D = 12
# number of measurement params for self._chassis_kf
# 4-D measurement: vx, vy, vz, wz_wheels
self._chassis_M = 4
# no control vars for self._chassis_kf
# Mecanum wheel odom vars
self.GEAR_RATIO = 19.0
self.WHEEL_RADIUS = 0.075
# To be set by derived classes, differs from robot to robot
# Length between front mecanum wheels and back mecanum wheels
self.L = None
# Width between right mecanum wheels and left mecanum wheels
self.W = None
# Matrix that defines the transformation of wheel RPM to
# chassis relative velocities. Differs from robot to robot
# Since it is reliant on self.L and self.W
self.MECANUM_M = None
self._imu_kf = None
# number of state params for self._imu_kf
# 9-D state: roll, pitch, yaw and first and second derivatives in that order
self._imu_D = 9
# number of measurement params for self._imu_kf
# 7-D measurement: roll, pitch, yaw and first derivatives + wz measured based on chassis measurements
self._imu_M = 7
# no control vars for self._imu_kf
def _publish_odom(self, timestamp):
'''
See base class comment
'''
# One of the kalman filters have not been initialized yet, don't publish
if not self._chassis_kf or not self._imu_kf:
return
if self._should_reset():
rospy.logwarn("Odometry diverged, resetting kalman filters...")
self._reset_kf()
return
odom_msg = Odometry()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "base_center" # fitting 2 frame transforms into 1 odom msg
odom_msg.header.stamp = timestamp
chassis_state = self._chassis_kf.statePost[:, 0]
imu_state = self._imu_kf.statePost[:, 0]
# Translation
(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y,
odom_msg.pose.pose.position.z) = chassis_state[0:3]
# Rotation
mod = 2 * math.pi
mcb_imu_quat = tf_conversions.transformations.quaternion_from_euler(
imu_state[0] % mod, imu_state[1] % mod, imu_state[2] % mod)
pose_quat = convert_quat_to_frame(mcb_imu_quat, "base_center",
"mcb_imu")
(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w) = pose_quat
# Set pose covariances
chassis_cov = self._chassis_kf.errorCovPost
imu_cov = self._imu_kf.errorCovPost
pose_cov_matrix = np.zeros((6, 6), dtype=np.float32)
pose_cov_matrix[0:3, 0:3] = chassis_cov[0:3, 0:3]
pose_cov_matrix[3:6, 3:6] = imu_cov[0:3, 0:3]
odom_msg.pose.covariance = tuple(pose_cov_matrix.flatten())
# Translational velocity
(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y,
odom_msg.twist.twist.linear.z) = chassis_state[4:7]
# Angular velocities
twist_euler = convert_angv_to_frame(imu_state[3:6], "base_center",
"mcb_imu")
(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y,
odom_msg.twist.twist.angular.z) = twist_euler[0:3]
# Set velocity covariances
twist_cov_matrix = np.zeros((6, 6), dtype=np.float32)
twist_cov_matrix[0:3, 0:3] = chassis_cov[4:7, 4:7]
twist_cov_matrix[3:6, 3:6] = imu_cov[3:6, 3:6]
odom_msg.twist.covariance = tuple(twist_cov_matrix.flatten())
self._odom_pub.publish(odom_msg)
def _publish_accel(self, timestamp):
'''
See base class comment
'''
# One of the kalman filters have not been initialized yet, don't publish
if not self._imu_kf or not self._chassis_kf:
return
accel_msg = AccelStamped()
accel_msg.header.frame_id = "odom"
accel_msg.header.stamp = timestamp
(accel_msg.accel.linear.x, accel_msg.accel.linear.y,
accel_msg.accel.linear.z) = self._chassis_kf.statePost[8:11, 0]
(accel_msg.accel.angular.x, accel_msg.accel.angular.y,
accel_msg.accel.angular.z) = self._imu_kf.statePost[6:9, 0]
self._accel_pub.publish(accel_msg)
def _init_chassis_kf(self, initial_meas):
'''
Initializes self._chassis_kf to 0 position and acceleration and initial
velocity based on the initial measurements.
@params initial_meas - (4, 1) np.array world frame relative [[vx], [vy], [vz], [wz_chassis]]
'''
self._chassis_kf = KalmanFilter(dynamParams=self._chassis_D,
measureParams=self._chassis_M)
# transitionMatrix will be edited before each predict/correct step with coefficients for the delta_t
self._chassis_kf.transitionMatrix = np.eye(
self._chassis_D,
dtype=np.float32) # transitionMatrix @ state_n = state_n+1
self._chassis_kf.measurementMatrix = np.array(
[ # predictedMeasurements += measurementMatrix @ statePre_n
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], # vx
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], # vy
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], # vz
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], # wz_wheels
],
np.float32)
self._chassis_kf.processNoiseCov = np.diag(
np.array([
0.05**2, 0.05**2, 0.05**2, 0.05**2, 0.1**2, 0.1**2, 0.1**2, 0.1
**2, 0.01**2, 0.01**2, 0.01**2, 0.01**2
],
dtype=np.float32))
self._chassis_kf.measurementNoiseCov = 0.005 * np.eye(self._chassis_M,
dtype=np.float32)
# initial state
# opencv KalmanFilter initializes both statePre and statePost to 0 proper vectors (i.e. 2 dims)
vel_start = 4
vel_end = vel_start + self._chassis_M
self._chassis_kf.statePre[vel_start:vel_end,
0] = initial_meas[:self._chassis_M, 0]
self._chassis_kf.statePost[vel_start:vel_end,
0] = initial_meas[:self._chassis_M, 0]
def _init_imu_kf(self, initial_meas):
'''
Initializes self._imu_kf to 0 angular position and acceleration and initial
velocity based on the initial measurements.
@params initial_meas - (7, 1) np.array IMU frame relative
[[roll], [pitch], [yaw], [wx], [wy], [wz], [yaw based on chassis yaw]]
'''
self._imu_kf = KalmanFilter(dynamParams=self._imu_D,
measureParams=self._imu_M)
# transitionMatrix will be edited before each predict/correct step with coefficients for the delta_t
self._imu_kf.transitionMatrix = np.eye(
self._imu_D,
dtype=np.float32) # transitionMatrix @ state_n = state_n+1
self._imu_kf.measurementMatrix = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
],
dtype=np.float32)
# guessed covariances.
self._imu_kf.processNoiseCov = np.diag(
np.array([
0.05**2, 0.05**2, 0.05**2, 0.1**2, 0.1**2, 0.1**2, 0.01**2,
0.01**2, 0.01**2
],
dtype=np.float32))
self._imu_kf.measurementNoiseCov = np.diag(
np.array([
0.005**2, 0.005**2, 0.05**2, 0.01**2, 0.01**2, 0.01**2, 0.0075
**2
],
dtype=np.float32))
# initial state
self._imu_kf.statePre[:self._imu_M, 0] = initial_meas[:self._imu_M, 0]
self._imu_kf.statePost[:self._imu_M, 0] = initial_meas[:self._imu_M, 0]
def _configure_chassis_kf_for_update(self, delta_t):
'''
Configures self._chassis_kf for the predict step based on the time
difference between last measurement and current measurement
as part of the extended Kalman Filter
@params delta_t. Time difference between last measurement and current measurement
'''
vel = delta_t # velocity/2nd-derivative factor
acc = 0.5 * delta_t**2
np.fill_diagonal(self._chassis_kf.transitionMatrix[:8, 4:], vel)
np.fill_diagonal(self._chassis_kf.transitionMatrix[:4, 8:], acc)
def _configure_imu_kf_for_update(self, delta_t):
'''
Configures self._imu_kf for the predict step based on the time
difference between last measurement and current measurement
as part of the extended Kalman Filter
@params delta_t. Time difference between last measurement and current measurement
'''
vel = delta_t # velocity/2nd-derivative factor
acc = 0.5 * delta_t**2
np.fill_diagonal(self._imu_kf.transitionMatrix[:6, 3:], vel)
np.fill_diagonal(self._imu_kf.transitionMatrix[:3, 6:], acc)
def update(self, imu_stamped):
'''
Updates the current odometry state of this object by
carrying out the predict and update step on self._chassis_kf
and self._imu_kf
@params imu_stamped - the McbOdomMessage ROS message that contains the raw
sensor data from the MCB
'''
current_timestamp = imu_stamped.header.stamp.to_sec()
# Must update chassis first and then IMU
# bec imu KF's yaw_wheels measurement uses
# chassis statePost's yaw_wheels estimate to compute
# an additional measurement for yaw based on wheels yaw to correct IMU yaw drift
# Chassis update
# Convert from RPM to radians/s
# See: https://dacemirror.sci-hub.tw/journal-article/7eee4f02b97da1bba632b6e57aa7856e/conduraru2014.pdf
rf = -(imu_stamped.rf_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
lf = (imu_stamped.lf_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
lb = (imu_stamped.lb_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
rb = -(imu_stamped.rb_rpm / self.GEAR_RATIO * 2 * math.pi) / 60
velocities = np.matmul(self.MECANUM_M,
np.array([[rf], [lf], [lb], [rb]]))
# Sum and convert to world relative
# now working in ros coordinates
vx_chassis = velocities[1, 0]
vy_chassis = -velocities[0, 0]
# Chassis predict: if KF init already, use statePre's yaw to compute
# robot vx and vy in static frame
# else use chassis_yaw = 0
# Stretch TODO: slope odometry
chassis_yaw = 0
if self._chassis_kf:
self._configure_chassis_kf_for_update(
current_timestamp -
self._last_update_timestamp.to_sec() if self.
_last_update_timestamp else 0)
self._chassis_kf.predict()
chassis_yaw = self._chassis_kf.statePre[3, 0]
vx = vx_chassis * math.cos(chassis_yaw) - vy_chassis * math.sin(
chassis_yaw)
vy = vx_chassis * math.sin(chassis_yaw) + vy_chassis * math.cos(
chassis_yaw)
wz_wheels = velocities[2, 0]
current_chassis_meas = np.array([[vx], [vy], [0], [wz_wheels]],
np.float32)
if not self._chassis_kf:
self._init_chassis_kf(current_chassis_meas)
self._configure_chassis_kf_for_update(0)
self._chassis_kf.predict()
self._chassis_kf.correct(current_chassis_meas)
# IMU update
current_imu_meas = np.array(
[[imu_stamped.rol], [imu_stamped.pit], [imu_stamped.yaw],
[imu_stamped.wx], [imu_stamped.wy], [imu_stamped.wz], [0]],
np.float32)
if not self._imu_kf:
self._init_imu_kf(current_imu_meas)
self._configure_imu_kf_for_update(
current_timestamp - self._last_update_timestamp.to_sec() if self.
_last_update_timestamp else 0)
self._imu_kf.predict()
# Convert yaw wheels readings from base_center frame to IMU frame
# to use as an additional measurement for the yaw in order to correct IMU yaw drift
predicted_trans_quat = tf_conversions.transformations.quaternion_from_euler(
self._imu_kf.statePre[0, 0], self._imu_kf.statePre[1, 0],
self._imu_kf.statePre[2, 0])
predicted_trans_quat[3] = -predicted_trans_quat[3]
yaw_wheels = convert_euler_to_quat_frame(
(0, 0, self._chassis_kf.statePost[3, 0]), predicted_trans_quat)[2]
current_imu_meas[6, 0] = yaw_wheels
#print(current_imu_meas)
self._imu_kf.correct(current_imu_meas)
#print(self._imu_kf.statePost[2,0], self._imu_kf.statePost[5, 0], self._imu_kf.statePost[8, 0])
#with open("/home/nvidia/our-log.csv", "a+") as f:
#f.write(str(current_timestamp) + "," + str(self._chassis_kf.statePost[0, 0]) + "," + str(self._chassis_kf.statePost[1, 0]) + "," + str(self._chassis_kf.statePost[2, 0]) + "\n")
self._last_update_timestamp = imu_stamped.header.stamp
def _should_reset(self):
'''
See base class comment
'''
if self._imu_kf and self._chassis_kf:
abs_imu_state = np.abs(self._imu_kf.statePost)
abs_chassis_state = np.abs(self._chassis_kf.statePost)
abs_imu_error = np.abs(self._imu_kf.errorCovPost)
abs_chassis_error = np.abs(self._chassis_kf.errorCovPost)
return (np.any(np.isnan(abs_imu_error))
or np.any(np.isnan(abs_chassis_error))
or np.any(abs_imu_state[3:, :] > 100)
or np.any(abs_chassis_state[4:, :] > 100))
return False
def _reset_kf(self):
'''
Clears the 2 Kalman Filters' states
'''
self._chassis_kf = None
self._imu_kf = None