Hercules Autopilot University of Florida TI Innovation Challenge 015 Project Report Team Leader: Team Members: Advising Professor: Video Mentor (if applicable): Jordan Street <jordantstreet@gmail.com> N/A Fred Taylor <fjt@ece.ufl.edu> http://www.youtube.com/watch?v=wy8l0aoxrkg N/A Date: 5/4/015 Qty. List all TI analog IC and TI processor part number and URL 1) Explain where it was used in the project? ) What specific features or performance made this component wellsuited to the design? 1 RM46L85 This microcontroller (MCU) runs all of the flight control software on the air module. It was chosen for the following reasons: High performance this MCU can run at 0 MHz and features 1.66 DMIPS/MHz with a doubleprecision hardware floating point unit (FPU). The increased computing power is necessary to run the flight control algorithms, specifically the Extended Kalman Filter (EKF). Safety features this MCU has physical CPUs running in lock step with the ability to signal the event of an error. Memory this MCU features 1.5 MB of flash providing plenty of room for future expansion of the flight control software. The 19 KB of RAM is enough to accommodate the flight software calculations. Peripherals this MCU features enough pulse width modulation (PWM) inputs and outputs to interface with standard RC receivers and control up to 8 servos or electronic speed controllers (ESCs). An IC line is provided to interface with an external digital compass. Two UART lines are provided to interface with a USBUART bridge and an external GPS. Multiple SPI ports are provided to interface with sensors and an EEPROM. 1 TPS7A7001 This low dropout (LDO) regulator provides the core 3.3V rail used by the air module. This part was chosen due to its low dropout voltage, relatively high current rating, input voltage range and available packaging. 1 LP3879 This LDO provides the core 1.V rail used by the RM46L85. This part was chosen due to its low dropout voltage, input voltage range and available packaging. 1 TPS3106K33 This supervisory circuit holds the RM46L85 in reset until the core 1.V rail has stabilized. This is the recommended power up sequence found in the RM46L85 TRM. TPD4E1U06 This transient voltage suppressor (TVS) is used to protect the USB circuitry against electrostatic discharge (ESD)
damage. It is found on both the air module and telemetry module USB lines. TPS553 USB devices are allowed to draw 100mA during normal operation and 500mA if they are approved for high current. This device controls the current limit depending on the enumeration status of the device. It is found in both the air module and telemetry module USB circuits. 1 TPS05B This power distribution switch provides a mechanism to independently enable or disable the 5V rails for the external SPI and GPS ports. This device was chosen due to its input voltage range, current rating, and available packaging. 3 SN74LVC8T45 This bus transceiver is used to buffer communications lines on the air module. This device buffers the (PWM) inputs and outputs along with the external GPS and SPI communications lines. The device was chosen due to its input voltage range and available packaging. 1 PCA9515B This IC bus repeater is used to buffer the IC communications lines for the digital compass. It was chosen due to its max operating frequency and available packaging. 1 SN74LVC3G07 This buffer is used to drive the high current RGB LED used for status indication on the air module. It was chosen due to its input voltage range, current sinking ability, and available packaging. The Hercules Autopilot is a full flight control system designed for quadrotor unmanned aerial vehicles (UAVs) featuring 100% custom designed hardware and software. Both manual flight control modes and fully autonomous waypoint navigation are supported. The system consists of an air module which is mounted on the flight vehicle and a telemetry module which interfaces with commercial ground station software on a laptop. A radio controlled (RC) transmitter is used to fly the quadrotor in manual flight modes and switch to autonomous waypoint navigation. The telemetry module provides the current flight conditions as well as mission status. Quadrotor UAVs are unstable systems which require flight controllers to fly. Unlike an RC plane which can be controlled by directly interfacing the RC receiver with the control surface servos, a quadrotor must have additional hardware to translate flight commands to appropriate motor speeds. The Hercules Autopilot features two primary flight modes, angle mode (or manual mode) and waypoint mode (autonomous mode). In angle mode, the pilot uses the right stick of the RC transmitter to command tilt angles resulting in translational motion. Vertical movement of the left stick controls the overall throttle command. Horizontal movement of the left stick controls the vehicle rotation rate command. In waypoint mode, the quadrotor flies a predetermined flight path specified by GPS waypoints completely autonomously.
015 Innovation Challenge Hercules Autopilot Jordan Street Faculty Advisor: Fred Taylor University of Florida
Hercules Autopilot 015 TI Innovation Challenge Page Table of Contents Table of Contents... List of Figures... Project Overview...3 Hardware Design...4 Air Module...4 Telemetry Module...7 Software Design...8 Guidance...8 Navigation...8 Control...9 Results...10 Future Work...10 References...11 Appendix A...1 Appendix B...16 List of Figures Figure 1 Hercules Autopilot air module interfaced with quadrotor UAV...3 Figure RC transmitter...3 Figure 3 Hercules Autopilot telemetry module interfaced with laptop running ground station software...3 Figure 4 Quadrotor "X" configuration...3 Figure 5 Hercules Autopilot Air Module (Top)...4 Figure 6 Hercules Autopilot Air Module (Bottom)...4 Figure 7 Hercules Autopilot Air Module Block Diagram...5 Figure 8 Air Module PCB (Top Layer)...6 Figure 9 Air Module PCB (Ground Plane)...6 Figure 10 Air Module PCB (Split Power Plane)...6 Figure 11 Air Module PCB (Bottom Layer)...6 Figure 1 Hercules Autopilot Telemetry Module (Top)...7 Figure 13 Hercules Autopilot Telemetry Module (Bottom)...7 Figure 14 Hercules Autopilot Telemetry Module Block Diagram...7 Figure 15 Telemetry Module PCB (Top Layer)...7 Figure 16 Telmetry Module PCB (Ground Plane 1)...7 Figure 17 Telemetry Module PCB (Ground Plane )...7 Figure 18 Telemetry Module PCB (Bottom Layer)...7 Figure 19 Hercules Autopilot Navigation Block Diagram...8 Figure 0 Angle Mode Controller...9 Figure 1 Waypoint Mode Controller... 9
Hercules Autopilot 015 TI Innovation Challenge Page 3 Project Overview The Hercules Autopilot is a full flight control system designed for quadrotor unmanned aerial vehicles (UAVs) featuring 100% custom designed hardware and software. Both manual flight control modes and fully autonomous waypoint navigation are supported. The system consists of an air module which is mounted on the flight vehicle and a telemetry module which interfaces with commercial ground station software on a laptop. A radio controlled (RC) transmitter is used to fly the quadrotor in manual flight modes and switch to autonomous waypoint navigation. The telemetry module provides the current flight conditions as well as mission status. Shown below are all components of the system. Figure 1 Hercules Autopilot air module interfaced with quadrotor UAV Figure RC transmitter Figure 3 Hercules Autopilot telemetry module interfaced with laptop running ground station software Quadrotor UAVs are unstable systems which require flight controllers to fly. Unlike an RC plane which can be controlled by directly interfacing the RC receiver with the control surface servos, a quadrotor must have additional hardware to translate flight commands to appropriate motor speeds. The diagram below illustrates the basic quadrotor setup. Figure 4 Quadrotor "X" configuration Hover is achieved by increasing the speed of all motors until the net thrust balances the vehicle weight. Motors 1 and 3 (M1, M3) spin clockwise (CW) while motors and 4 (M, M4) spin counterclockwise (CCW) to zero the net torque. Translational motion is achieved by tilting the vehicle such that a
Hercules Autopilot 015 TI Innovation Challenge Page 4 component of the net thrust is in the desired direction of travel. The vehicle can rotate to the left or right by increasing and decreasing opposite pairs of motors. This maintains the same net thrust with a nonzero net torque producing rotation. To move the vehicle forward, motors 3 and 4 are increased while motors 1 and are decreased. This forward tilt causes a component of the net thrust to be in the forward direction. To move the vehicle to the right, motors 1 and 4 are increased while motors and 3 are decreased. This tilts the vehicle to the right causing a component of the net thrust to be in the right direction. To rotate the vehicle to the right, motors and 4 are increased while motors 1 and 3 are decreased. This nonzero net torque rotates the vehicle to the right. Note that the net torque is opposite the direction of the dominant motor rotation. This is because the torque is primarily due to propeller drag which acts opposite the direction of rotation. The Hercules Autopilot features two primary flight modes, angle mode (manual mode) and waypoint mode (autonomous mode). In angle mode, the pilot uses the right stick of the RC transmitter to command tilt angles resulting in translational motion. Vertical movement of the left stick controls the overall throttle command. Horizontal movement of the left stick controls the vehicle rotation rate command. In waypoint mode, the quadrotor autonomously flies a predetermined flight path specified by GPS waypoints. Hardware Design The Hercules Autopilot system consists of an air module and telemetry module. Each module is discussed in detail below. Air Module The air module is responsible for controlling the flight vehicle. It includes a comprehensive sensor package with ports for a GPS and digital compass (magnetometer). An RF link is included to communicate wirelessly with the telemetry module on the ground. The power system is tripleredundant to mitigate power related risks. All critical inputs and outputs are buffered to improve signal integrity. The onboard ARM CortexR4F Hercules processor provides a stable platform for safety critical applications. The following figure shows the assembled air module. Figure 5 Hercules Autopilot Air Module (Top) Figure 6 Hercules Autopilot Air Module (Bottom)
Hercules Autopilot 015 TI Innovation Challenge Page 5 The following block diagram illustrates all major components and connections in the air module. 5V (main) 3.3V (main) 3.3V LDO TPS7A7001 Altimeter Meas. Spec. MS5611 Gyro & Accel InvenSense MPU6000 EEPROM ON Semiconductor CAT518 3.3V 1.V (main) 1.V LDO LP3879 SPI SPI SPI GPIO Ext. GPS/SPI Power En TPS05B Ext. SPI 5V GPS 5V 1.V RF Link (ground station) USB (computer) POR Manager TPS3106 RF Module Murata DNT4C USBUART Bridge FTDI FT30XS RST UART (jumper routing) GPIO (power path status) Autopilot MCU RM46L85 (ARM CortexR4F) PWM PWM GPIO (LED indication) IC UART SPI GPIO IC Buffer PCA9515B Ext. Comms Buffer SN74LVC8T45 RGB Buffer SN74LVC3G07 IC Ext. GPS & Compass Ublox & Honeywell NEO7M & HMC5883L UART Ext. SPI RGB LED 5V USB ESD Protection TPD4E1U06 Servo Buffer SN74LVC8T45 Receiver Buffer SN74LVC8T45 5V 5V (main) PWM PWM USB Current Limit TPS553 5V Power Path Controller Linear Technology LTC4417 5V Speed Controller Turnigy Plush 4A RC Receiver Spektrum AR610 RF Link (RC transmitter) Ext. 5V Battery Figure 7 Hercules Autopilot Air Module Block Diagram The air module consists of the following major subsystems. Power Supply the Linear Technology LTC4417 power path controller allows for automatic prioritized switching between three distinct power sources. The air module can be powered from the USB connection, the 5V electronic speed controller (ESC), or a dedicated battery. With all three power sources connected, the air module is tripleredundant and can seamlessly switch sources in the event of a power failure. The TI TPS7A7001 and TI LP3879 lowdropout (LDO) regulators provide core 3.3V and 1.V rails for the system. The TI TPS3106 supervisory circuit holds the autopilot microcontroller (MCU) in reset until the core 1.V rail has stabilized. The TI TPD4E1U06 transient voltage suppressor (TVS) protects against electrostatic discharge (ESD) on the USB lines. The TI TPS553 adjustable current limited switch limits the USB current depending on the USB enumeration status and high current approval. The TI TPS05B power
Hercules Autopilot 015 TI Innovation Challenge Page 6 distribution switch allows the 5V power rails for the external GPS/compass and SPI ports to be enabled or disabled separately. Onboard Sensors the air module includes a Measurement Specialties MS5611 barometric altimeter for altitude measurements. The InvenSense MPU6000 3axis gyroscope and 3axis accelerometer provides high bandwidth inertial data used in the inertial navigation system (INS). External Sensors separate ports are provided to interface with an external GPS and digital compass. An external SPI port is provided for future expansion. The TI SN74LVC8T45 bus transceiver buffers both the GPS UART and external SPI port communication lines. The TI PCA9515B IC repeater buffers the digital compass IC communication lines. Pulse Width Modulation (PWM) Input & Output the air module includes 6 PWM inputs for interfacing with the RC receiver and 8 PWM outputs for interfacing with servos or ESCs. All PWM inputs and outputs are buffered by TI SN74LVC8T45 bus transceivers. USB Debug an FTDI FT30XS USBUART bridge allows the autopilot MCU or onboard RF module to communicate with a USB host through UART. Selection between the MCU and RF module is done through a set of jumpers. RF Link the Murata DNT4C RF module provides a proven RF solution for wireless communication between the air module and telemetry module. The RF link can be connected through UART to either the USB debug host or the autopilot MCU through a set of jumpers. Autopilot MCU the TI RM46L85 Hercules microcontroller is a high performance ARM CortexR4F safety MCU. It operates at 0 MHz and features an extensive set of peripherals and safety features. The MCU interfaces with all of the previously discussed peripherals in addition to an ON CAT518 EEPROM used for nonvolatile parameter storage. A TI SN74LVC3G07 triple buffer is used to drive a high current RGB LED for status indication. The air module PCB is shown below. For additional details, the schematic is provided in Appendix B. Figure 8 Air Module PCB (Top Layer) Figure 9 Air Module PCB (Ground Plane) Figure 10 Air Module PCB (Split Power Plane) Figure 11 Air Module PCB (Bottom Layer)
Hercules Autopilot 015 TI Innovation Challenge Page 7 Telemetry Module The Telemetry module serves as a bridge between the air vehicle and the ground station running on a laptop. It includes an RF link to communicate with the air module and a USBUART bridge to communicate with the laptop. The following figure shows the assembled telemetry module. Figure 1 Hercules Autopilot Telemetry Module (Top) Figure 13 Hercules Autopilot Telemetry Module (Bottom) The following block diagram illustrates all major components and connections in the telemetry module. UART USB (computer) USBUART Bridge FTDI FT30XS RF Module Murata DNT4C RF Link (air module) 5V 5V USB ESD Protection TPD4E1U06 5V USB Current Limit TPS553 Figure 14 Hercules Autopilot Telemetry Module Block Diagram The Murata DNT4C RF module allows the telemetry module to wirelessly communicate with the air module. The FTDI FT30XS USBUART bridge allows the onboard RF module to communicate with a USB host through UART. The TI TPD4E1U06 transient voltage suppressor (TVS) protects against electrostatic discharge (ESD) on the USB lines. The TI TPS553 adjustable current limited switch limits the USB current depending on the USB enumeration status and high current approval. The telemetry module PCB is shown below. For additional details, see the schematic in Appendix B. Figure 15 Telemetry Module PCB (Top Layer) Figure 16 Telmetry Module PCB (Ground Plane 1) Figure 17 Telemetry Module PCB (Ground Plane ) Figure 18 Telemetry Module PCB (Bottom Layer)
Hercules Autopilot 015 TI Innovation Challenge Page 8 Software Design All flight control software runs on the autopilot MCU (TI RM46L85) onboard the air module. The flight control software is broken up into three primary functions: guidance, navigation, and control. Each of these functions is discussed in detail in the following sections. Guidance The guidance section is responsible for issuing flight commands to be carried out by the control section. Depending on the flight mode, different commands are generated. In angle mode (manual mode), the guidance section receives the RC transmitter stick positions and commands angular positions and rates. Vertical movement of the right stick controls the commanded pitch angle (θ) of the vehicle. Horizontal movement of the right stick controls the commanded roll angle (φ) of the vehicle. Horizontal movement of the left stick controls the commanded yaw rate (r). Vertical movement of the left stick is passed through the guidance section to the control section and influences the total thrust generated by the vehicle. In waypoint mode (autonomous mode), the guidance section generates velocity and angular rate commands. A heading is calculated based on the current position and next waypoint. The difference between this heading and the current yaw angle is calculated as the heading error. The heading error is then translated into a yaw rate command. A forward velocity inversely proportional to the heading error is commanded. The difference in altitude between the current position and next waypoint is calculated as the altitude error. This altitude error is translated into a vertical velocity command. Navigation The navigation section is responsible for determining the vehicle position and orientation in the northeastdown (NED) frame. An Extended Kalman Filter (EKF) is used to fuse information from multiple sensors and calculate an estimate of the current vehicle state. The following block diagram illustrates the state estimation process. For details on the EKF formalization see Appendix A. Gyro Biases 3Axis Rate Gyro Quaternion Diff. Eq. Angular Position Magnetic North DCM i>b 3Axis DCM Accelerometer b>i Accelerometer Biases Kalman Gain Angular Position Innovation X Angular Position Velocity (NED) Position (NED) State Dynamics State Estimates Angular Position Gyro Biases Velocity (NED) Position (NED) Accelerometer Biases Velocity (NED) Position (NED) N&E velocity Measurement Model 3Axis Magnetometer GPS Barometric Altimeter GPS N&E Velocity GPS N&E Position Altitude Innovation Figure 19 Hercules Autopilot Navigation Block Diagram
Hercules Autopilot 015 TI Innovation Challenge Page 9 Control The control section is responsible for carrying out the flight commands issued by the guidance section. The following figure illustrates the control loops used in angle mode. Roll PID Roll Rate PID Roll Command Roll Angle Roll Rate Pitch Command Pitch PID Pitch Rate PID Quadrotor Motor Mixing Motor PWM Pitch Angle Pitch Rate lim Yaw Rate PID Yaw Rate Command Yaw Rate Throttle Command Figure 0 Angle Mode Controller The estimated roll and pitch angles are subtracted from the commanded roll and pitch angles to form the roll and pitch error. The roll and pitch PID controllers translate these errors into roll rate and pitch rate commands. The estimated roll and pitch rates are subtracted from the commanded roll and pitch rates to form the roll rate and pitch rate error. The estimated yaw rate is subtracted from the commanded yaw rate to form the yaw rate error. The roll rate, pitch rate, and yaw rate PID controllers translate these errors into angular acceleration commands. These commands are mixed with the throttle according to the motor mixing block to create motor speed commands. The following figure illustrates the control loops used in waypoint mode. Y Velocity Command Y Velocity PID Y Velocity Y Velocity Yaw Angle DCM i>b X Velocity PID Roll Command Angle Mode Controller X Velocity Command X Velocity Pitch Command Yaw Rate Command Motor PWM X Velocity (NED) Yaw Angle DCM i>b Throttle Command Z Velocity PID Z Velocity Command Z Velocity Z Velocity Trim Figure 1 Waypoint Mode Controller
Hercules Autopilot 015 TI Innovation Challenge Page 10 Waypoint mode adds additional control loops around the angle mode controller. Velocity commands from the guidance section are given in a headingfixed NED frame. In this frame, the projection of the body xaxis on the horizontal northeast plane is always aligned with north. This allows velocities to be commanded relative to the vehicle. A positive X velocity command always corresponds to commanded motion in the vehicles forward direction. Positive Y velocity corresponds to motion to the right of the vehicle. The estimated Y and X velocities are subtracted from the commanded Y and X velocities to form the Y and X velocity errors. The Y Velocity and X Velocity PID controllers translate these errors into roll and pitch commands. The yaw rate command generated form the guidance section is passed through to the angle mode controller. The estimated Z velocity is subtracted from the Z velocity command to form the Z velocity error. The Z velocity PID controller translates this error into a throttle command. The Z velocity trim centers the PID output about the throttle hover condition. Results The hardware and software solutions detailed above allow the Hercules Autopilot to successfully fly a quadrotor UAV in both angle and waypoint modes. The submitted video shows demonstration flights of both flight modes. The vehicle is flown aggressively in angle mode to demonstrate the robustness of the flight software. A quartermile route with multiple waypoints and altitudes is flown completely autonomously in waypoint mode to demonstrate the flight software s ability to maneuver the vehicle along a predetermined path without operator intervention. Future Work Future work on the Hercules Autopilot is primarily concentrated in the following areas. Hardware future hardware modifications include small PCB changes to the air and telemetry modules as well as modifying some of the USB and LED status circuitry on the air module. Guidance the current guidance law nulls the heading error and commands a forward velocity. More advanced guidance laws would use a vectored velocity command to implement coordinated turns. Additional work could be done to improve path following robustness. Inertial Navigation System the INS precision could be enhanced by increasing the inertial sensor sample rate. The EKF prediction step would be run at this increased rate improving the state estimate precision. Ground Station the ability to add, edit, or delete waypoints directly from the ground station could be added. This would allow for autonomous missions to be edited inflight.
Hercules Autopilot 015 TI Innovation Challenge Page 11 References Barton, J. (n.d.). Fundamentals of Small Unmanned Aircraft Flight. Retrieved from http://www.jhuapl.edu/techdigest/td/td310/31_0barton.pdf Bouabdallah, S., & Siegwart, R. (n.d.). Full Control of a Quadrotor. Retrieved from Swiss Federal Institute of Technology Autonomous Systems Lab: http://publications.asl.ethz.ch/files/bouabdallah07full.pdf Christophersen, H., Pickell, W., Neidhoefer, J., Koller, A., Kannan, S., & Johnson, E. (006). A Compact Guidance, Navigation, and Control System for. JOURNAL OF AEROSPACE COMPUTING, INFORMATION, AND COMMUNICATION. Ozyagcilar, T. (n.d.). Implementing a TiltCompensated ecompass using Accelerometer and Magnetometer Sensors. Retrieved from http://cache.freescale.com/files/sensors/doc/app_note/an448.pdf QuaternionBased Quadrotor Control Simplified Quadrotor Model. (n.d.). Retrieved from UAVDesign.net: http://www.uavdesign.net/adaptiveadistributedcontrol/quaternionbasedquadrotorcontrol/?showall=&start=1 Schinstock, D. (n.d.). GPSaided INS Solution for. Retrieved from https://wiki.openpilot.org/download/attachments/097177/insgpsalg.pdf
Hercules Autopilot 015 TI Innovation Challenge Page 1 Appendix A This appendix details the Extended Kalman Filter used for state estimation in the navigation section. The following variables are used in the filter process model. x State vector u Input vector q s Quaternion s component ω x xaxis angular rate q x Quaternion x component ω y yaxis angular rate q y Quaternion y component ω z zaxis angular rate q z Quaternion z component a x xaxis acceleration b ωx Gyro xaxis bias a y yaxis acceleration b ωy Gyro yaxis bias a z zaxis acceleration b ωz Gyro zaxis bias f(x, u) Nonlinear state dynamics p x x position (local NED) F(x, u) Linear state dynamics p y y position (local NED) Q State covariance p z z position (local NED) σ q Quaternion variance v x x velocity (local NED) σ bω Gyro bias variance v y y velocity (local NED) σ p Position variance v z z velocity (local NED) σ v Velocity variance b ax Accelerometer xaxis bias σ ba Accelerometer bias variance b ay Accelerometer yaxis bias DCM b i DCM (body to inertial) b az Accelerometer zaxis bias g Gravity The state vector x and input vector u are given below. x = [q s q x q y q z b ωx b ωy b ωz p x p y p z v x v y v z b ax b ay b az ] T The nonlinear state dynamics are given below. u = [ω x ω y ω z a x a y a z] T 1 Ω ωq 0 x = f(x, u) = v { DCM b i [a b a ] g 0 }
Hercules Autopilot 015 TI Innovation Challenge Page 13 An expanded form of the previous differential equation is given below. x = f(x, u) = { 1 0 (ω x b ωx ) (ω y b ωy ) (ω z b ωz ) ω x b ωx 0 ω z b ωz (ω y b ωy ) ω y b ωy (ω z b ωz ) 0 ω x b ωx [ ω z b ωz ω y b ωy (ω x b ωx ) 0 ] [ q s q x q ] y q z 0 0 0 v x v y v z 1 (q y q z ) (q x q y q z q s ) (q x q z q y q s ) a x b ax 0 [ (q x q y q z q s ) 1 (q x q z ) (q y q z q x q s )] [ a y b ay ] [ 0] (q x q z q y q s ) (q y q z q x q s ) 1 (q x q y ) a z b az g 0 0 0 } In order to propagate the state estimate covariance through the Kalman filter, the Jacobian of the previous state dynamics must be calculated. The block form of this equation is given below. F(x, u) = F q,q F q,bω 0 0 0 0 0 0 0 0 0 0 0 I 3 3 0 Each entry of the previous block matrix is given below. F v,q 0 0 0 F v,ba [ 0 0 0 0 0 ] 0 (ω x b ωx ) (ω y b ωy ) (ω z b ωz ) F q,q = 1 ω x b ωx 0 ω z b ωz (ω y b ωy ) ω y b ωy (ω z b ωz ) 0 ω x b ωx [ ω z b ωz ω y b ωy (ω x b ωx ) 0 ] q x q y q z F q,bω = 1 [ q s q z q y q z q s q ] x q y q x q s
Hercules Autopilot 015 TI Innovation Challenge Page 14 a x a x b ax [ a y ] = [ a y b ay ] a z a z b az F v,q = q y a z q z a y q y a y q z a z q s a z q y a x q x a y q x a z q z a x q s a y [ q z a x q x a z q y a x q s a z q x a y q x a x q z a z q s a x q z a y q y a z ] q x a y q y a x q s a y q z a x q x a z q z a y q s a x q y a z q x a x q y a y q y q z 1 q s q z q x q y q s q y q x q z F v,ba = [ q s q z q x q y q x q z 1 q s q x q y q z ] q s q y q x q z q s q x q y q z q x q y 1 The process noise covariance matrix is given below. Note that the diagonal form of the covariance matrix implies independence between quantities. Q = [ σ q I 4 4 0 0 0 0 0 σ bω I 3 3 0 0 0 0 0 σ p I 3 3 0 0 0 0 0 σ v I 3 3 0 0 0 0 0 σ ba I 3 3 ] The following variables are used in the measurement model. z Measurement vector h(x) Nonlinear measurement model m x Magnetometer xaxis H(x) Linear measurement model m y Magnetometer yaxis DCM i b DCM (inertial to body) m z Magnetometer zaxis R Measurement covariance x gps GPS x position (local NED) σ m Magnetometer variance y gps GPS y position (local NED) σ pgps GPS position variance z alt Altimeter z position (local NED) σ alt Altimeter variance x gps GPS x velocity (local NED) σ v gps GPS velocity variance y gps GPS y velocity (local NED) B x Magnetic field x component B y Magnetic field y component Magnetic field z component The input vector z is given as follows. z = [m x m y m z x gps y gps z alt x gps y gps ] T B z
Hercules Autopilot 015 TI Innovation Challenge Page 15 The measurement model is given below. z = h(x) = { DCM i b [B] p v x v y } An expanded form of the previous equation is given as follows. z = h(x) = { 1 (q y q z ) (q x q y q z q s ) (q x q z q y q s ) B x B y [ (q x q y q z q s ) 1 (q x q z ) (q y q z q x q s )] [ ] (q x q z q y q s ) (q y q z q x q s ) 1 (q x q y ) B z p x p y p z v x v y } In order to propagate the state estimate covariance through the Kalman filter, the Jacobian of the previous measurement model must be calculated. The block form of this equation is given below. H m,q 0 0 0 0 H(x) = [ 0 0 I 3 3 0 0] 0 0 0 H vgps,v 0 H m,q = B y q z B z q y B y q y B z q z B y q x B x q y B z q s B y q s B x q z B z q x [ B z q x B x q z B z q s B x q y B y q x B x q x B z q z B z q y B y q z B x q S ] B x q y B y q x B x q z B y q s B z q x B x q s B y q z B z q y B x q x B y q y H vgps,v = [ 1 0 0 0 1 0 ] The measurement noise covariance matrix is given below. Note that the diagonal form of the covariance matrix implies independence between quantities. R = [ σ m I 3 3 0 0 0 0 σ pgps I 0 0 0 0 0 σ alt 0 0 0 σ v I gps ]
Hercules Autopilot 015 TI Innovation Challenge Page 16 The following variables are used in the Extended Kalman Filter equations. t x x P P y S K Sample period Apriori state estimate Aposteriori state estimate Apriori state estimate covariance Aposteriori state estimate covariance Innovation Innovation covariance Kalman gain The EKF prediction equations are given below for the continuous process model. x = f(x, u) P = F(x, u)p PF(x, u) T Q A first order Euler method is used to integrate the differential equations. This formulation is given below. P t1 x t1 = x t t f(x t, u t ) = P t t [F(x t, u t )P t P t F(x t, u t ) T Q] Since the measurement model is best described with discrete updates, the EKF update equations are given in their standard form as follows. y t1 = z t1 h(x t1 ) S t1 = H(x t1 P t1 K t1 = P t1 )P t1 H(x t1 ) T R H(x t1 ) T 1 S t1 x t1 = x t1 K t1 y t1 = [I K t1 H(x t1 )]P t1 Appendix B The following pages include schematics for both the Hercules Autopilot air module and telemetry module.
Hercules Autopilot Sensors Revision 0 Jordan Street University of Florida
Hercules Autopilot Servo/Receiver/Status Revision 0 Jordan Street University of Florida
Hercules Autopilot Telemetry/Debug Revision 0 Jordan Street University of Florida
Hercules Autopilot POR/ADC/Misc Revision 0 Jordan Street University of Florida
Hercules Autopilot Power Supply Revision 0 Jordan Street University of Florida
Hercules Autopilot Ground Telemetry Revision 0 Jordan Street University of Florida