Algorithm 2. Kalman Filter

Input:${\mathit{m}}_{\mathit{k}}$,  % object position for time step t from sensor

Output: ${\widehat{\mathit{s}}}_{\mathit{k}}$,  % a position estimation of object

1: initialize t, ${\widehat{\mathit{s}}}_{\mathit{k}-\mathbf{1}}$, A, P, Q, R  % t represents prediction time moment, ${\widehat{\mathit{s}}}_{\mathit{k}-\mathbf{1}}$ is the known posterior        % state estimation at time moment k − 1; A represents state transition matrix;        % P represents the covariance matrix, Q denotes covariance of the random        % signals, and R is the matrix of observation noise covariance.

2: if filterStop = false then  % end with convergence of click the ‘stop’ button.

3:  ${\widehat{\mathit{s}}}_{\mathit{k}}^{-}\leftarrow \mathit{A}{\widehat{\mathit{s}}}_{\mathit{k}-\mathbf{1}}$    % calculate predicting position estimation, according to Equation (16)

4:  P ← APAT + Q  % calculate priori covariance matrix, according to Equation (17)

5:  K ← PCT(CPCT + R)  % calculate Kalman Gain matrix, according to Equation (26)

6:  ${\widehat{\mathit{s}}}_{\mathit{k}}$ ← ${\widehat{\mathit{s}}}_{\mathit{k}}^{-}$ + K(${\mathit{m}}_{\mathit{k}}$ − C${\widehat{\mathit{s}}}_{\mathit{k}}^{-}$)  % calculate optimal estimation value, according to Equation (23)

7:  P ← (I − KC)P  % calculate ${\widehat{\mathit{x}}}_{\mathit{k}}$ covariance, according to Equation (27), I denotes unit matrix

8:  t ← t + 1

9: end if