In this algorithm, the observation vector is
obtained by TOF ranging and the TOF distance measurement
error is estimated rst. The state is estimated through the
algorithm in the end. From the entire process, it can be
observed that Oxk=k relates with the current input and previous
estimated state, so real time processing can be realized only
with the previous saved state value.