class IMUBarometerFusion(QMainWindow):
FACTOR = 1
KP1 = 0.55 * FACTOR # PI observer velocity gain
KP2 = 1.0 * FACTOR # PI observer position gain
KI = 0.001 / FACTOR # PI observer integral gain (bias cancellation)
def __init__(self, parent=None):
QMainWindow.__init__(self, parent)
self.initialized = False
self.altitude_error_i = 0
self.acc_scale = 0.0
self.start_time = time.time()
self.altitude_error = 0
self.inst_acceleration = 0.0
self.delta = 0
self.estimated_velocity = 0.0
self.estimated_altitude = 0.0
self.last_time = time.time()
self.last_orig_altitude = 0
self.last_estimated_altitude = 0
ipcon = IPConnection()
self.imu = IMU(UID_IMU, ipcon)
self.barometer = Barometer(UID_BAROMETER, ipcon)
ipcon.connect(HOST, PORT)
# Turn leds and orientation calculation off, to save calculation time
# for the IMU Brick. This makes sure that the measurements are taken
# in equidistant 2ms intervals
self.imu.leds_off()
self.imu.orientation_calculation_off()
# Turn averaging of in the Barometer Bricklet to make sure that
# the data is without delay
self.barometer.set_averaging(0, 0, 0)
red_pen = QPen(Qt.red)
red_pen.setWidth(5)
plot_list = [ #['', Qt.blue, self.get_orig_value],
['', red_pen, self.get_estimated_value]
]
self.plot_widget = PlotWidget('Height [m]', plot_list)
self.plot_widget.stop = False
self.setCentralWidget(self.plot_widget)
self.timer = QTimer()
self.timer.timeout.connect(self.update)
self.timer.start(6)
def get_orig_value(self):
return self.last_orig_altitude
def get_estimated_value(self):
return self.last_estimated_altitude
# Update measurements and compute new altitude every 6ms.
def update(self):
q = self.imu.get_quaternion()
acc = self.imu.get_acceleration()
alt = self.barometer.get_altitude() / 100.0
compensated_acc_q = self.compute_compensated_acc(q, acc)
compensated_acc_q_earth = self.compute_dynamic_acceleration_vector(
q, compensated_acc_q)
self.last_orig_altitude = alt
self.last_estimated_altitude = self.compute_altitude(
compensated_acc_q_earth[2], alt)
# Remove gravity from accelerometer measurements
def compute_compensated_acc(self, q, a):
g = (2 * (q.x * q.z - q.w * q.y), 2 * (q.w * q.x + q.y * q.z),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)
return (a[0] / 1000.0 - g[0], a[1] / 1000.0 - g[1],
a[2] / 1000.0 - g[2], 0.0)
# Rotate dynamic acceleration vector from sensor frame to earth frame
def compute_dynamic_acceleration_vector(self, q, compensated_acc_q):
def q_conj(q):
return -q[0], -q[1], -q[2], q[3]
def q_mult(q1, q2):
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return x, y, z, w
tmp = q_mult(q, compensated_acc_q)
return q_mult(tmp, q_conj(q))
# Computes estimation of altitude based on barometer and accelerometer measurements
# Code is based on blog post from Fabio Varesano: http://www.varesano.net/blog/fabio/complementary-filtering-high-res-barometer-and-accelerometer-reliable-altitude-estimation
# He seems to have got the idea from the MultiWii project
def compute_altitude(self, compensated_acceleration, altitude):
self.current_time = time.time()
# Initialization
if not self.initialized:
self.initialized = True
self.estimated_altitude = altitude
self.estimated_velocity = 0
self.altitude_error_i = 0
# Estimation Error
self.altitude_error = altitude - self.estimated_altitude
self.altitude_error_i = self.altitude_error_i + self.altitude_error
self.altitude_error_i = min(2500.0, max(-2500.0,
self.altitude_error_i))
self.inst_acceleration = compensated_acceleration * 9.80665 + self.altitude_error_i * self.KI
dt = self.current_time - self.last_time
# Integrators
self.delta = self.inst_acceleration * dt + (self.KP1 *
dt) * self.altitude_error
self.estimated_altitude += (self.estimated_velocity / 5.0 +
self.delta) * (dt / 2) + (
self.KP2 * dt) * self.altitude_error
self.estimated_velocity += self.delta * 10.0
self.last_time = self.current_time
return self.estimated_altitude
class IMUBarometerFusion(QMainWindow):
FACTOR = 1
KP1 = 0.55*FACTOR # PI observer velocity gain
KP2 = 1.0*FACTOR # PI observer position gain
KI = 0.001/FACTOR # PI observer integral gain (bias cancellation)
def __init__(self, parent=None):
QMainWindow.__init__(self, parent)
self.initialized = False
self.altitude_error_i = 0
self.acc_scale = 0.0
self.start_time = time.time()
self.altitude_error = 0
self.inst_acceleration = 0.0
self.delta = 0
self.estimated_velocity = 0.0
self.estimated_altitude = 0.0
self.last_time = time.time()
self.last_orig_altitude = 0
self.last_estimated_altitude = 0
ipcon = IPConnection()
self.imu = IMU(UID_IMU, ipcon)
self.barometer = Barometer(UID_BAROMETER, ipcon)
ipcon.connect(HOST, PORT)
# Turn leds and orientation calculation off, to save calculation time
# for the IMU Brick. This makes sure that the measurements are taken
# in equidistant 2ms intervals
self.imu.leds_off()
self.imu.orientation_calculation_off()
# Turn averaging of in the Barometer Bricklet to make sure that
# the data is without delay
self.barometer.set_averaging(0, 0, 0)
red_pen = QPen(Qt.red)
red_pen.setWidth(5)
plot_list = [#['', Qt.blue, self.get_orig_value],
['', red_pen, self.get_estimated_value]
]
self.plot_widget = PlotWidget('Height [m]', plot_list)
self.plot_widget.stop = False
self.setCentralWidget(self.plot_widget)
self.timer = QTimer()
self.timer.timeout.connect(self.update)
self.timer.start(6)
def get_orig_value(self):
return self.last_orig_altitude
def get_estimated_value(self):
return self.last_estimated_altitude
# Update measurements and compute new altitude every 6ms.
def update(self):
q = self.imu.get_quaternion()
acc = self.imu.get_acceleration()
alt = self.barometer.get_altitude()/100.0
compensated_acc_q = self.compute_compensated_acc(q, acc)
compensated_acc_q_earth = self.compute_dynamic_acceleration_vector(q, compensated_acc_q)
self.last_orig_altitude = alt
self.last_estimated_altitude = self.compute_altitude(compensated_acc_q_earth[2], alt)
# Remove gravity from accelerometer measurements
def compute_compensated_acc(self, q, a):
g = (2*(q.x*q.z - q.w*q.y),
2*(q.w*q.x + q.y*q.z),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z)
return (a[0]/1000.0 - g[0],
a[1]/1000.0 - g[1],
a[2]/1000.0 - g[2],
0.0)
# Rotate dynamic acceleration vector from sensor frame to earth frame
def compute_dynamic_acceleration_vector(self, q, compensated_acc_q):
def q_conj(q):
return -q[0], -q[1], -q[2], q[3]
def q_mult(q1, q2):
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return x, y, z, w
tmp = q_mult(q, compensated_acc_q)
return q_mult(tmp, q_conj(q))
# Computes estimation of altitude based on barometer and accelerometer measurements
# Code is based on blog post from Fabio Varesano: http://www.varesano.net/blog/fabio/complementary-filtering-high-res-barometer-and-accelerometer-reliable-altitude-estimation
# He seems to have got the idea from the MultiWii project
def compute_altitude(self, compensated_acceleration, altitude):
self.current_time = time.time()
# Initialization
if not self.initialized:
self.initialized = True
self.estimated_altitude = altitude
self.estimated_velocity = 0
self.altitude_error_i = 0
# Estimation Error
self.altitude_error = altitude - self.estimated_altitude
self.altitude_error_i = self.altitude_error_i + self.altitude_error
self.altitude_error_i = min(2500.0, max(-2500.0, self.altitude_error_i))
self.inst_acceleration = compensated_acceleration * 9.80665 + self.altitude_error_i * self.KI
dt = self.current_time - self.last_time
# Integrators
self.delta = self.inst_acceleration * dt + (self.KP1 * dt) * self.altitude_error
self.estimated_altitude += (self.estimated_velocity/5.0 + self.delta) * (dt / 2) + (self.KP2 * dt) * self.altitude_error
self.estimated_velocity += self.delta*10.0
self.last_time = self.current_time
return self.estimated_altitude
