SHAKF-PU: Sage–Husa Adaptive Kalman Filtering-Based Pedestrian Characteristic Parameter Update Mechanism for Enhancing Step Length Estimation in Pedestrian Dead Reckoning

Algorithm 2

SHAKF-PU.

Input: Z, X, K, e, P, d, q, Q, r, R, t

Output: X

(1) Xold ← X

(2) Pold ← P

(3) Xf ← F × X + B × q

(4) Pf ← F ×  F + B × Q × B

(5) K ← Pf × H/(H × Pf × H + R)

(6) e ← Z−H × Xf−r

(7) X ← Xf + K × e

(8) P ← (I−K × H) × Pf

(9) d ← (1−b)/(1−pow (b, t + 1))

(10) q ← (1−d) × q + d × (X–F × Xold)

(11) Q ← (1−d) × Q + d × (K × e × e × K + P-F × Pold × F)

(12) r ← (1−d) × r + d × (Z−H × Xf)

(13) R ← (1−d) × R + d×(e × e−H × Pf × H)

(14) return X