Robust Plug-and-Play Joint Axis Estimation Using Inertial Sensors

. 2020 Jun 22;20(12):3534. doi: 10.3390/s20123534

Algorithm 1 Joint axis estimation

Require: Data

D^{N} = {y_{ω}^{N}, y_{a}^{N}}

, initial estimate

\hat{x} (0)

, tolerance

V_{tol}

, residual weights

w_{ω}

and

w_{a}

1:
for $i \in {1, 2}$ do
2:
$k \leftarrow 0$ . ▹ Begin Gauss–Newton.
3:
$Δ_{V} \leftarrow V_{tol}$ .
4:
$V (0) \leftarrow V (\hat{x} (0))$ . ▹ $V (x)$ defined by (23).
5:
while $Δ_{V} \geq V_{tol}$ do
6:
Compute the Jacobian $J (\hat{x} (k))$ and the residuals $e (\hat{x} (k))$ according to (31) and (32).
7:
$Δ_{x} (k) \leftarrow {(J^{⊤} (\hat{x} (k)) J (\hat{x} (k)))}^{- 1} J^{⊤} (\hat{x} (k)) e (\hat{x} (k))$ .
8:
Obtain step length $α$ using backtracking line search.
9:
$\hat{x} (k + 1) \leftarrow \hat{x} (k) - α Δ_{x} (k)$ .
10:
$k \leftarrow k + 1$ .
11:
$V (k) \leftarrow V (\hat{x} (k))$ .
12:
$Δ_{V} \leftarrow | V (k - 1) - V (k) |$ .
13:
end while
14:
$\hat{x} \leftarrow \hat{x} (k)$ . ▹ End Gauss–Newton.
15:
${\hat{x}}^{(i)} = {[\begin{matrix} {\hat{θ}}_{1}^{(i)} & {\hat{ϕ}}_{1}^{(i)} & {\hat{θ}}_{2}^{(i)} & {\hat{ϕ}}_{2}^{(i)} \end{matrix}]}^{⊤} \leftarrow \hat{x}$ .
16:
$\hat{x} (0) \leftarrow {[\begin{matrix} {\hat{θ}}_{1}^{(i)} & {\hat{ϕ}}_{1}^{(i)} & - {\hat{θ}}_{2}^{(i)} & {\hat{ϕ}}_{2}^{(i)} + π \end{matrix}]}^{⊤}$ . ▹ Initialize at $- {\hat{j}}_{2}$ .
17:
end for
18:
$\hat{x} \leftarrow {arg min}_{x \in {{\hat{x}}^{(1)}, {\hat{x}}^{(2)}}} V (x)$ . ▹Correct sign pairing.
19:
return $j (\hat{x})$ .