Fixed the linear array accessors in stateToImu to properly fill the
covariance matrix