AP_Math: added vectorN class, and index checking
AP_NavEKF: use vectorN to make indexes safe