AP_NavEKF : Add focal length scale factor estimation