Matlab Code For Smoothing Ins Gps
- K * H) * P_pred; end % Backward RTS smoothing x_smooth = x; P_smooth = P; for k = N-1:-1:1 A = P(:,:,k) * F' / (F * P(:,:,k) * F' + Q); x_smooth(:,k) = x(:,k) + A * (x_smooth(:,k+1) - F * x(:,k)); P_smooth(:,:,k) = P(:,:,k