def test_pitch_xz_acceleration(self):
time = 123.023
accelerometers = [0.0, 0.0, -9.81]
gyros = [0.0, 1.0, 0.0]
imu = ImuMsg(time, accelerometers, gyros)
dt = 0.1
estimator = Estimator()
estimator.imuPrevTime = time
steps = 5
for i in range(steps):
imu.time = estimator.imuPrevTime + dt
pPrev = estimator.belief.p
qPrev = estimator.belief.q
vPrev = estimator.belief.v
baPrev = estimator.belief.ba
bgPrev = estimator.belief.bg
estimator.imu_callback(imu)
positionEcef = [-1800198.22956286, -4532912.78883003, 4098399.82768237]
velocityEcef = [0.0, 0.0, 0.0]
gps = GpsMsg(positionEcef, velocityEcef)
estimator.set_ref_lla_callback(40.23, -111.66, 1370.0)
estimator.gps_callback(gps)
print('accelerometer bias = ', estimator.belief.ba)
print('gyro bias = ', estimator.belief.bg)
def test_orientation_angular_position_velocity(self):
t = 0.0 #not important
position = [1.3,43.2,-6.5]
orientation = [1.2,-0.3,2.2]
velocity = [3.3,-0.4,-2.1]
angularVelocity = [2.3,-0.1,0.44]
acceleration = [0.0,0.0,0.0]
truth = TruthMsg(t,position,orientation,velocity,acceleration,angularVelocity)
imu = ImuMsg()
accelerometers = [-2.8991,-8.7349,-3.3960]
gyros = [2.3,-0.1,0.44]
expectedImu = ImuMsg(t,accelerometers,gyros)
synthetic_measurements.compute_imu(truth,imu)
self.assert_imus(imu,expectedImu)
def test_zeros(self):
t = 0.0 #not important
position = [0.0,0.0,0.0]
orientation = [0.0,0.0,0.0]
velocity = [0.0,0.0,0.0]
angularVelocity = [0.0,0.0,0.0]
acceleration = [0.0,0.0,0.0]
truth = TruthMsg(t,position,orientation,velocity,angularVelocity,acceleration)
imu = ImuMsg()
accelerometers = [0.0,0.0,-9.81]
gyros = [0.0,0.0,0.0]
expectedImu = ImuMsg(t,accelerometers,gyros)
synthetic_measurements.compute_imu(truth,imu)
self.assert_imus(imu,expectedImu)
def test_random(self):
t = 0.0 #not important
position = [-3.0,1.0,-1.0]
orientation = [0.2,0.3,-0.1]
velocity = [-2.96377917, 1.48202406, 0.14006332]
angularVelocity = [0.0,0.0,0.0]
acceleration = [0.0,0.0,0.0]
truth = TruthMsg(t,position,orientation,velocity,angularVelocity,acceleration)
imu = ImuMsg()
accelerometers = [2.89905323, -1.86189936, -9.1850379]
gyros = [0.0,0.0,0.0]
expectedImu = ImuMsg(t,accelerometers,gyros)
synthetic_measurements.compute_imu(truth,imu)
self.assert_imus(imu,expectedImu)
def test_roll_yz_acceleration(self):
time = 123.023
accelerometers = [0.0, 0.0, -9.81]
gyros = [1.0, 0.0, 0.0]
imu = ImuMsg(time, accelerometers, gyros)
dt = 0.1
estimator = Estimator()
estimator.imuPrevTime = time
steps = 5
for i in range(steps):
imu.time = estimator.imuPrevTime + dt
pPrev = estimator.belief.p
qPrev = estimator.belief.q
vPrev = estimator.belief.v
baPrev = estimator.belief.ba
bgPrev = estimator.belief.bg
estimator.imu_callback(imu)
self.check_roll_trend(estimator.belief, pPrev, qPrev, vPrev,
baPrev, bgPrev)
def imuCallback(self, msg):
timeSeconds = msg.header.stamp.secs + msg.header.stamp.nsecs * 1E-9
gyrosRadiansPerSecond = [
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z
]
accelerometersMetersPerSecondSquared = [
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
]
imu = ImuMsg(timeSeconds, accelerometersMetersPerSecondSquared,
gyrosRadiansPerSecond)
self.estimator.imu_callback(imu)
self.publish_estimates()
def test_same_inputs(self):
p0 = np.zeros((3, 1))
q0 = np.array(([[0.0, 0.5036, 0.0]])).T
v0 = np.zeros((3, 1))
ba0 = np.zeros((3, 1))
bg0 = np.zeros((3, 1))
cov0 = np.array([[1.0, 1.0, 1.0]]).T
belief = StatesCovariance(p0, q0, v0, ba0, bg0, cov0, cov0, cov0, cov0,
cov0)
accelerometers = [-4.905, 0.0, -8.496]
gyros = [0.0, 0.2, 0.0]
imu = ImuMsg(0.0, accelerometers, gyros)
dt = 0.1
kp = 0.1
ki = 0.9
comp_filter.run(belief, imu, dt, kp, ki)
qExpected = np.array([[0.0, 0.0, 0.0]]).T
self.assert_attitude(belief.q, qExpected)
def test_zeros(self):
p = np.zeros((3, 1))
q = np.zeros((3, 1))
v = np.zeros((3, 1))
ba = np.zeros((3, 1))
bg = np.zeros((3, 1))
cov = np.identity(15)
belief = StatesCovariance(p, q, v, ba, bg, cov)
accelerometers = [0.0, 0.0, -9.81]
gyros = [0.0, 0.0, 0.0]
ut = ImuMsg(0.0, accelerometers, gyros)
ft = ekf.update_dynamic_model
At = ekf.calculate_numerical_jacobian_A(ft, belief, ut)
AtExpected = np.array(
[[0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -1., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -1., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -1.],
[
0., 0., 0., 0., -9.809837, 0., 0., 0., 0., -1., 0., 0., 0.,
0., 0.
],
[
0., 0., 0., 9.809837, 0., 0., 0., 0., 0., 0., -1., 0., 0., 0.,
0.
],
[
0., 0., 0., -0.04905, -0.04905, 0., 0., 0., 0., 0., 0., -1.,
0., 0., 0.
], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]])
self.assert_jacobian_equal(At, AtExpected)
def __init__(self):
self.truthRos = Odometry()
self.imuRos = Imu()
self.relPosRos = RelPos()
self.baseGpsRos = PosVelEcef()
self.roverGpsRos = PosVelEcef()
self.rtkCompassRos = RelPos()
self.truth = TruthMsg()
self.roverTruth = TruthMsg()
self.imu = ImuMsg()
self.base2RoverRelPos = RelPosMsg()
self.baseGps = GpsMsg()
self.roverGps = GpsMsg()
self.rtkCompass = GpsCompassMsg
self.accelerometerAccuracyStdDev = 0.025
self.gyroAccuracyStdDev = 0.0023
self.gpsHorizontalAccuracyStdDev = 0.2
self.gpsVerticalAccuracyStdDev = 0.4
self.gpsSpeedAccuracyStdDev = 0.2
self.rtkHorizontalAccuracyStdDev = 0.02
self.rtkVerticalAccuracyStdDev = 0.04
self.rtkCompassAccuracyDegStdDev = 1.0 #depends on baseline. This also uses RTK
#These are to remember noise of previous update. They are used in the low pass filter
self.gpsNoise = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.rtkCompassNoise = 0.0
self.accelerometerBias = [0.3, -0.5, -0.2]
self.gyroBias = [0.01, 0.08, -0.02]
self.imuTs = 1.0 / 200.0
self.gpsTs = 1.0 / 5.0
self.firstCallback = True
self.firstTime = 0.0
self.latRef = 20.24
self.lonRef = -111.66
self.altRef = 1387.0
self.originEcef = navpy.lla2ecef(self.latRef, self.lonRef, self.altRef)
self.gravity = np.array([[0.0, 0.0, 9.81]]).T
self.lowPassFilterAlpha = 0.9
self.truth_pub_ = rospy.Publisher('base_truth',
Odometry,
queue_size=5,
latch=True)
self.imu_pub_ = rospy.Publisher('base_imu',
Imu,
queue_size=5,
latch=True)
self.relPos_pub_ = rospy.Publisher('base2Rover_relPos',
RelPos,
queue_size=5,
latch=True)
self.base_gps_pub_ = rospy.Publisher('base_gps',
PosVelEcef,
queue_size=5,
latch=True)
self.rover_gps_pub_ = rospy.Publisher('rover_gps',
PosVelEcef,
queue_size=5,
latch=True)
self.rtk_compass_pub_ = rospy.Publisher('base_rtk_compass',
RelPos,
queue_size=5,
latch=True)
self.truth_rate_timer_ = rospy.Timer(rospy.Duration(self.imuTs),
self.truthCallback)
self.gps_rate_timer_ = rospy.Timer(rospy.Duration(self.gpsTs),
self.gpsCallback)
while not rospy.is_shutdown():
rospy.spin()