High-Speed Hardware Design of Kalman Filter Based on FPU with Formula Derivation Method