Robust Self-Contained Pedestrian Navigation by Fusing the IMU and Compass Measurements via UFIR Filtering

Algorithm 2

UFIR filter algorithm for the self-contained scheme.

Data: , ,

Result:

(1) begin

(2)for do

(3)

(4)

(5)

(6)

(7)if then

(8)

(9)

(10)end if

(11)if then

(12)

(13)end if

(14)for do

(15)

(16)

(17)

(18)

(19)end for

(20)

(21)end for

(22) end

(23)    is the state vector size

(24)    is the filtering horizon size