Table - PMC

Skip to main content

View full-text article in PMC

. 2021 May 14;21(10):3428. doi: 10.3390/s21103428

Algorithm 2 Proposed Kalman-based filter with velocity observation measurement update.

1:
System Initialization.
2:
Load Initial Values.
3:
Compute State transition matrix.
4:
Compute Prior state $x_{k}^{-}$ .
5:
Compute Prior error covariance.
6:
Integrate quaternion $q_{k}$ .
7:
if (30) is satisfied (GM available) then
8:
Calculate the residual (17).
9:
if (31) is satisfied (ZARU update available) then
10:
Update the residual (21).
11:
end if
12:
Calculate Kalman gain, Update Error covariance.
13:
Calculate state $x_{k}$ .
14:
Update quaternion $q_{k}$ with (6).
15:
end if
16:
if $k \in P_{H}$ (Step is detected) then
17:
if (29) is satisfied ( ${\bar{n}}_{v, d i f f} \approx 0$ ) then
18:
Calculate the residual (24).
19:
Calculate Kalman gain, Update Error covariance.
20:
Calculate state $x_{k}$ .
21:
Update quaternion $q_{k}$ with (6).
22:
end if
23:
end if
24:
Goto 3.