예제 #1
0
파일: polaris.py 프로젝트: motor411/polaris
 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()
예제 #2
0
 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()
예제 #3
0
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),
        }
예제 #4
0
파일: polaris.py 프로젝트: motor411/polaris
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),
        }