def __init__(self, imu, differential_pressure_sensor,
static_pressure_sensor, magnetometer, gps):
self.phi, self.theta, self.psi = 0, 0, 0
self.Vair = 0.0
self.attitude_observer = AttitudeObserver()
self.heading_observer = HeadingObserver()
self.altitude_observer = AltitudeObserver()
self.wind_observer = WindObserver()
self.position_observer = PositionObserver()
# Comming soon:
# self.navigation_observer = NavigationObserver()
self.imu = imu
self.differential_pressure_sensor = differential_pressure_sensor
self.static_pressure_sensor = static_pressure_sensor
self.magnetometer = magnetometer
self.gps = EmulatedXplaneGPS(delay=1.1, Hz=2)
self.last_time = datetime.now()
def __init__(self, imu, differential_pressure_sensor, static_pressure_sensor, magnetometer, gps):
self.phi, self.theta, self.psi = 0, 0, 0
self.Vair = 0.0
self.attitude_observer = AttitudeObserver()
self.heading_observer = HeadingObserver()
self.altitude_observer = AltitudeObserver()
self.wind_observer = WindObserver()
self.position_observer = PositionObserver()
# Comming soon:
# self.navigation_observer = NavigationObserver()
self.imu = imu
self.differential_pressure_sensor = differential_pressure_sensor
self.static_pressure_sensor = static_pressure_sensor
self.magnetometer = magnetometer
self.gps = EmulatedXplaneGPS(delay=1.1, Hz=2)
self.last_time = datetime.now()
class Polaris:
"""
Polaris is a six-degree-of-freedom extended Kalman filter for inertial navigation, and *requires* a 3-axis gyro, 3-axis accelerometer, a 3-axis magnetometer, an airspeed sensor, an altimeter and GPS. It is based on the work in this paper: http://contentdm.lib.byu.edu/ETD/image/etd1527.pdf
States tracked (or to be tracked):
* Yaw, pitch, roll
* Airspeed, ground speed
* Altitude
* Position
* Wind vector
* Battery state
* What else?
"""
def __init__(self, imu, differential_pressure_sensor, static_pressure_sensor, magnetometer, gps):
self.phi, self.theta, self.psi = 0, 0, 0
self.Vair = 0.0
self.attitude_observer = AttitudeObserver()
self.heading_observer = HeadingObserver()
self.altitude_observer = AltitudeObserver()
self.wind_observer = WindObserver()
self.position_observer = PositionObserver()
# Comming soon:
# self.navigation_observer = NavigationObserver()
self.imu = imu
self.differential_pressure_sensor = differential_pressure_sensor
self.static_pressure_sensor = static_pressure_sensor
self.magnetometer = magnetometer
self.gps = EmulatedXplaneGPS(delay=1.1, Hz=2)
self.last_time = datetime.now()
def loop(self):
TD.DT = (datetime.now()-self.last_time).microseconds/1000000.0
self.last_time = datetime.now()
p, q, r = self.imu.read_gyros()
ax, ay, az = self.imu.read_accelerometers()
Vair = self.differential_pressure_sensor.read_airspeed() * 0.5144444444 # kias to meters per second
#display.register_scalars({"Vair":Vair/0.5144444444}, "Sensors")
phi, theta = self.attitude_observer.estimate_roll_and_pitch(p, q, r, Vair, ax, ay, az, TD.DT)
bx, by, bz = self.magnetometer.read_magnetometer()
psi = self.heading_observer.estimate_heading(bx, by, bz, phi, theta, q, r, TD.DT)
gps_data = self.gps.update(TD.DT)
#display.register_scalars({"gps_lat": gps_data['latitude'],
# "gps_lon": gps_data['longitude'],
# "gps_alt": gps_data['altitude'],
# "gps_sog": gps_data['speed_over_ground'],}, "Sensors")
altitude = self.altitude_observer.estimate(theta, Vair, gps_data['altitude'], TD.DT)
#display.register_scalars({"alt_est": altitude}, "Estimates")
#display.register_scalars({"Altitude": altitude - TD.ALTITUDE}, "Performance")
wind_direction, wind_velocity = self.wind_observer.estimate(theta, psi, Vair, gps_data['speed_over_ground'] * .5144444444, gps_data['course_over_ground'], TD.DT)
GPS_Pn, GPS_Pe = self.gps.relative_gps()
X = self.position_observer.estimate(Vair, theta, psi, GPS_Pn, GPS_Pe, TD.DT)
Pn, Pe, Wn, We, Cpress = X[0,0], X[1,0], X[2,0], X[3,0], X[4,0]
wind_direction = degrees(atan2(We,Wn))
wind_velocity = sqrt(We**2 + Wn**2) # * 0.592483801 # convert from ft/s to knots
display.register_scalars({"Pn": Pn,"Pe": Pe,"Wn": Wn,"We": We,"GPS_Pn":GPS_Pn,"GPS_Pe":GPS_Pe,"Cpress":Cpress}, "Dead reckoning")
display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity}, "Dead reckoning")
#display.register_scalars({"Wdir error": wind_direction - TD.WIND_DIRECTION, "Wvel error": wind_velocity - TD.WIND_VELOCITY}, "Performance")
# Shoot, I forget what this is, something from Ryan
# //SOG[0] = 'data from gps'
# //u_dot = 'x accel from a2d'
# gamma = theta - avg_aoa;
# v = 0;
# w = airspeed*(sin(theta)*cos(gamma) - cos(theta)*sin(gamma));
# lin_accel_x = u_dot - (g*(-sin(theta)) - (r*v - q*w);
# SOG[0] += dt_gps*lin_accel_x;
return {
"roll": degrees(phi),
"pitch": degrees(theta),
"yaw": degrees(psi),
# Coming soon:
# "airspeed": Vair,
# "position": (position_north, position_east),
# "wind": (wind_north, wind_east),
}
class Polaris:
"""
Polaris is a six-degree-of-freedom extended Kalman filter for inertial navigation, and *requires* a 3-axis gyro, 3-axis accelerometer, a 3-axis magnetometer, an airspeed sensor, an altimeter and GPS. It is based on the work in this paper: http://contentdm.lib.byu.edu/ETD/image/etd1527.pdf
States tracked (or to be tracked):
* Yaw, pitch, roll
* Airspeed, ground speed
* Altitude
* Position
* Wind vector
* Battery state
* What else?
"""
def __init__(self, imu, differential_pressure_sensor,
static_pressure_sensor, magnetometer, gps):
self.phi, self.theta, self.psi = 0, 0, 0
self.Vair = 0.0
self.attitude_observer = AttitudeObserver()
self.heading_observer = HeadingObserver()
self.altitude_observer = AltitudeObserver()
self.wind_observer = WindObserver()
self.position_observer = PositionObserver()
# Comming soon:
# self.navigation_observer = NavigationObserver()
self.imu = imu
self.differential_pressure_sensor = differential_pressure_sensor
self.static_pressure_sensor = static_pressure_sensor
self.magnetometer = magnetometer
self.gps = EmulatedXplaneGPS(delay=1.1, Hz=2)
self.last_time = datetime.now()
def loop(self):
TD.DT = (datetime.now() - self.last_time).microseconds / 1000000.0
self.last_time = datetime.now()
p, q, r = self.imu.read_gyros()
ax, ay, az = self.imu.read_accelerometers()
Vair = self.differential_pressure_sensor.read_airspeed(
) * 0.5144444444 # kias to meters per second
#display.register_scalars({"Vair":Vair/0.5144444444}, "Sensors")
phi, theta = self.attitude_observer.estimate_roll_and_pitch(
p, q, r, Vair, ax, ay, az, TD.DT)
bx, by, bz = self.magnetometer.read_magnetometer()
psi = self.heading_observer.estimate_heading(bx, by, bz, phi, theta, q,
r, TD.DT)
gps_data = self.gps.update(TD.DT)
#display.register_scalars({"gps_lat": gps_data['latitude'],
# "gps_lon": gps_data['longitude'],
# "gps_alt": gps_data['altitude'],
# "gps_sog": gps_data['speed_over_ground'],}, "Sensors")
altitude = self.altitude_observer.estimate(theta, Vair,
gps_data['altitude'], TD.DT)
#display.register_scalars({"alt_est": altitude}, "Estimates")
#display.register_scalars({"Altitude": altitude - TD.ALTITUDE}, "Performance")
wind_direction, wind_velocity = self.wind_observer.estimate(
theta, psi, Vair, gps_data['speed_over_ground'] * .5144444444,
gps_data['course_over_ground'], TD.DT)
GPS_Pn, GPS_Pe = self.gps.relative_gps()
X = self.position_observer.estimate(Vair, theta, psi, GPS_Pn, GPS_Pe,
TD.DT)
Pn, Pe, Wn, We, Cpress = X[0, 0], X[1, 0], X[2, 0], X[3, 0], X[4, 0]
wind_direction = degrees(atan2(We, Wn))
wind_velocity = sqrt(
We**2 + Wn**2) # * 0.592483801 # convert from ft/s to knots
display.register_scalars(
{
"Pn": Pn,
"Pe": Pe,
"Wn": Wn,
"We": We,
"GPS_Pn": GPS_Pn,
"GPS_Pe": GPS_Pe,
"Cpress": Cpress
}, "Dead reckoning")
display.register_scalars(
{
"wind_direction": wind_direction,
"wind_velocity": wind_velocity
}, "Dead reckoning")
#display.register_scalars({"Wdir error": wind_direction - TD.WIND_DIRECTION, "Wvel error": wind_velocity - TD.WIND_VELOCITY}, "Performance")
# Shoot, I forget what this is, something from Ryan
# //SOG[0] = 'data from gps'
# //u_dot = 'x accel from a2d'
# gamma = theta - avg_aoa;
# v = 0;
# w = airspeed*(sin(theta)*cos(gamma) - cos(theta)*sin(gamma));
# lin_accel_x = u_dot - (g*(-sin(theta)) - (r*v - q*w);
# SOG[0] += dt_gps*lin_accel_x;
return {
"roll": degrees(phi),
"pitch": degrees(theta),
"yaw": degrees(psi),
# Coming soon:
# "airspeed": Vair,
# "position": (position_north, position_east),
# "wind": (wind_north, wind_east),
}