Checking before acosf()

In arducopter v4.0.6 and AP_NavEKF3_core.cpp, in line 1716 and 1758, it is
‘’‘float t3 = acosf(q0);’’’
I think it is better to check if q0 is greater than 1 before feeding it to acosf, to make sure NaN is not generated.
Thanks in advance!

Can you create a github PR with that change?