I noticed these two lines in ekf_update and ekf_predict:
_transpose(H, Ht, EKF_M, EKF_N);
_transpose(F, Ft, EKF_N, EKF_N);
Aren't H and F constants? Or is it reasonable for them to change during run-time? If they're constant, then the transpose of those two matrixes can be saved at initialization time. Perhaps a #define would make sense for those that think the memory sacrifice is worth the speed-up?