High Speed Digital Systems Lab November 4 th , 2010

1. INS/GPS navigation INS/GPS navigation using a regularized particle filter Covariance block Finalpresentation Developer: Ayal Ozer Mentor: Michael Yampolsky High Speed Digital Systems Lab November 4th, 2010

2. INS/GPS navigationAgenda • Background • System Overview • Interface • Architecture • Difficulties and solutions • results • Summary

3. INS/GPS navigation Background • INS/GPS – Inertial navigation with GPS to prevent error divergence • 30K particles, each representing a probability of the object’s state • The particles’ variables are advanced every 10 milliseconds • Every 100 cycles a GPS reading reweighs the particles • If there is a particle degeneration they are resampled and regularized

4. INS/GPS navigationProject goals • Convert quaternions to Euler angles • Calculate the covariance matrix and its root matrix • Perform the above tasks as efficiently as possible, both in space and in time

5. INS/GPS navigationProject formulae • Angle conversion: • Discontinuity fix: • Covariance matrix: • Root matrix, defined by:

6. INS/GPS navigation Difficulties and solutions • Trigonometric functions • LUT ROM vs. Cordic or Taylor algorithms • The Cholesky decomposition • NIOS II core vs. hardware implementation

7. INS/GPS navigationSystem Inputs & Outputs INPUT • Particlevector: Xk1..N • Particle Weight: wk • The estimated state vector: Xk OUTPUT • Euler angles particlevectors: ξk1..N • The covariance root matrix: DkT ^

8. COV column weight root matrix INS/GPS navigationTop Level Architecture MAIN CONTROLLER particle in particle out FSM state vector debug data weight reduced particle particle in Covariance block Angle module Root matrix block Covariance block Angle module Root matrix block particle out

9. particle out particle in start finished INS/GPS navigationAngle Converter Architecture angle converter LOGIC discon-tinuity fix now avg sin-1() LUT tan-1() LUT

10. w INS/GPS navigation Cov. phase 1 ξ1 Mult 1 ξi ξ2 Mult 2 Weight Mult ξ17ξ2 ξ1 matrix column ξ17 Mult 17

11. INS/GPS navigation Cov. phase 2 next column Σ Column FIFO register column adder added column matrix column

12. INS/GPS navigation Root Matrix Architecture SOPC CPU start CPU finished matrix writer matrix reader CPU COV column root matrix cell Avalon BUS

13. INS/GPS navigationResults: accuracy • Error definition: • Initial average errors are very high • Fixed the input files, and ran new simulations that uses them • Final results – error average • T201 – 0.2% • T1001 – 1.9% • T2001 – 10.5% • Other comparisons gave greater errors

14. INS/GPS navigationData analysis • Conclusion #1: fixed point inadequate for ε values

15. INS/GPS navigationData analysis • Conclusion #2: Numerous errors found in input and output files make it hard to determine overall accuracy. • error averages increase in later point of simulation • Very low values give high errors, since the fixed point is discrete

16. INS/GPS navigationResults: Performance • Conversion and covariance time is: • 30K*23 cycles @50MHz = 13.8 [msec] • Root matrix calculation time is: • ~17M cycles @ 50MHz = 350[msec] • This is due to a 64bit integer type. If fixed, it should take ~2M cycles @ 50MHz = 40[msec]

17. INS/GPS navigationResults: Area • Logic utilization is 17% • DSP utilization: 35% • Memory utilization: 28% • This allows integration with resampling block • DSP: Covariance parallelization is limited • Memory: usage of CPU and two large LUT (96KB+48KB) is acceptable

18. INS/GPS navigationConclusions • Using the Nios processor was a simple solution but a slow one. Before choosing it, check timing • LUTs are a problematic solution, but with enough memory give good accuracy • The hardware and software models must be compatible • A multi team project relies on communication and cooperation