FPGA-based sensor fusion

Jeremy Soh
School of Aerospace, Mechanical & Mechatronic Engineering
FACULTY OF ENGINEERING & INFORMATION TECHNOLOGIES Abstract
Development of low cost nano-satellites emphasise the use of commercial-off-theshelf (COTS) components which leads to somewhat lackluster performance during state estimation, limiting their potential applications. In order to satisfy strict pointing requirements, there is a need for complex state estimation algorithms to compensate for low accuracy sensors, however traditional microcontroller/processor based approaches often lack the power required to meet the real-time computing deadlines. The feasibility of a direct to hardware implementation, based on a Field Programmable Gate Array (FPGA), of the Unscented Kalman Filter (UKF) for state estimation is investigated here; the design is verified via simulation. Predict Step Mean and covariance are recovered to predict the current state:

Supervisor: Dr. Xiaofeng Wu

Update Step The standard Kalman filter equations are used to update the state based on a new measurement, z, and measurement covariance, S :

Matlab Simulation
Simulated satellite motion as slowly oscillating, 5 on all three axes, n = 0.1 Hz Simulated measurements with a sample rate of 1 kHz

Motivation
High accuracy, real-time state estimation is important not just for satellites, but also for a wide range of autonomous systems including UAVs, autonomous ground vehicles, robotic arms etc.

FPGA Simulation
Same parameters as Matlab Simulation

UKF
State Estimation Sampling method to estimate the state model:

q, q0 are quarternions representing attitude is the angular rate is the gyroscopic bias Rest are process or measurement noise terms Unscented Transform (UT) deterministically draws samples, called sigma points, from the system covariance, Pa Process Model Sigma points are propagated through the system process, f, and observation, h, models:

Roll Mean () () -0.0033 0.0623

Pitch -0.0148 0.0735

Yaw 0.0510 0.0339

FPGA Synthesis Results

Resource Utilisation Xilinx Virtex-5
Resource Slice LUTs BRAMs I/O DSPs Used 15308 7 297 70 % Total 34 4 46 54

Timing Estimates Period (Intel i7 3.8 Ghz) : > 2 ms

FPGA
Arithmetic Issues Single precision floating point numerical representation Matrix square root computed using Cholesky decomposition Trigonometric functions implemented using CORDIC Matrix right divide to solve linear systems of equations used instead of computing matrix inverses directly Looping Issues Iterative statements cannot, in general, be synthesised into hardware Implemented via a series of counters with THIS RESEARCH IS SP internally generated clock and reset signals

Timing & Power Estimates Period (100 Mhz crystal): 115 us Minimum period: 202 ns Maximum frequency: 5 MHz Operating power: 92 mW

Conclusion
FPGA-based design exhibits similar accuracy to computer-based implementation but is much faster FPGA implementation is then not only viable but superior to micro-controller based design.