Added covariance initialisation
Added covariance initialisation

zlongshen/InertialNav,AlexHAHA/InertialNav,flyingk/InertialNav,flyingk/InertialNav,priseborough/InertialNav,ek99800/InertialNav,AlexHAHA/InertialNav,flyingk/InertialNav,priseborough/InertialNav,AlexHAHA/InertialNav,flyingk/InertialNav,zlongshen/InertialNav,zlongshen/InertialNav,vergil1874/InertialNav,vergil1874/InertialNav,priseborough/InertialNav,ek99800/InertialNav,vergil1874/InertialNav,ek99800/InertialNav,priseborough/InertialNav