A method for filtering spatial error from a measurement vector is provided to correct for roll, pitch and yaw angular motion. The method includes the following operations: Establish an unstabilized body reference frame. Convert the measurement vector to an unstabilized state vector xU in the unstabilized body reference frame. Establish a stabilized East-North-Up (ENU) reference frame. Calculate an unstabilized pre-transform covariance matrix MU from position variance of the body reference frame. Measure roll, pitch and yaw in the body reference frame as respective angle values (r, p, w). Calculate a transform matrix T between the body reference frame and the ENU reference frame. Calculate a stabilized data vector xS=TxU from the transform matrix and the unstabilized state vector. Calculate a measured angle error sensitivity matrix MA from the angle values. Calculate a tri-diagonal angle error component matrix ME with square values of angle variance of the body reference frame. Calculate a total error covariance matrix PS=MAMEMAT+TMUTT. Calculate a Kalman gain matrix for current time k+1 as K(k+1)=P(k+1|k)HT[HP(k+1|k)HT+PS]−1, where P(k+1|k) is predicted gain covariance matrix from previous time k to the current time (k+1), and H is measurement Jacobian. Finally, apply the Kalman gain matrix to a predicted state estimate for correcting the measurement vector xm.