Wiggling-Related Error Correction Method for Indirect ToF Imaging Systems

Abstract

Indirect time-of-flight (ToF) imaging systems enable a broad array of applications owing to their high frame rate, strong durability, and low cost. However, the wiggling-related error caused by the harmonics in the emitted signal significantly affects the range accuracy of indirect ToF imaging systems. In this paper, we establish a mathematical model of the wiggling-related error and propose a wiggling-related error correction method for indirect ToF imaging systems. This method adds a delay measurement and utilizes raw intensity measurements to evaluate the system state based on an adaptive Kalman filter (AKF), which is easy to implement in most indirect ToF imaging systems. Simulation and experimental results show that the proposed method performed well in reducing the wiggling-related error and had good robustness in different integration times. Compared with the existing methods, the proposed method not only has better performance but also is easier to implement. We believe that this study provides effective guidance for researchers understanding the wiggling-related error and a potential direction for the accuracy improvement of indirect ToF imaging systems.

1. Introduction

Over the past years, traditional 2D images have been unable to meet the demand for information perception, and 3D images have been widely studied and applied. Depth imaging systems, including stereo vision [1], structured light [2], and time-of-flight (ToF) [3], can be used to obtain 3D images. ToF imaging possesses the advantages of high real-time performance, strong durability, small size, and low cost. Therefore, this imaging system is suitable for a wide array of applications, such as robot navigation [4], security system [5], 3D reconstruction [6], precision farming [7], human-computer interaction [8], etc.

ToF imaging systems can be classified into direct ToF and indirect ToF imaging systems. Compared to direct ToF imaging systems, indirect ToF imaging systems have higher spatial resolution and are easier to implement [9,10]. Indirect ToF imaging systems provide range information by measuring the phase shift between the emitted and reflected sinusoidal amplitude modulated signals [11]. However, due to hardware limitations, non-sinusoidal amplitude modulated waveforms, such as square wave modulated signals, are typically used in indirect ToF imaging systems [12]. The unnecessary harmonics in the modulated signal will lead to a periodic systematic error known as the wiggling error [13]. On the other hand, the random error of range measurement caused by photon shot noise [14], called the wiggling random error in this paper, is also affected by the harmonics and presents a wiggling feature [15]. In this paper, the above two categories of errors are collectively referred to as the wiggling-related error. Figure 1 is a simulation of the wiggling-related error. As shown by the blue line in Figure 1, the wiggling error has four oscillations inside the unambiguous distance and reaches a value of up to ±143 mm. The maximum distance drift caused by the wiggling random error is more than 50 mm in the simulation (see Point B). In the actual measurement, the wiggling error and the wiggling random error are superimposed, which seriously impacts the range accuracy of ToF imaging systems. Thus, correcting the wiggling-related error for indirect ToF imaging systems is of great significance.

Much of the current research to correct or reduce the wiggling error is to use polynomial functions, look-up tables, B-spline functions, or damped sine functions to compensate for it [16]. Before the compensation, calibration experiments are required to evaluate the wiggling-related error, which is a time-consuming task. ToF imaging systems still need to be recalibrated if any system parameters change, e.g., the integration time, the modulation frequency, and the harmonic component. Beyond that, the amplitude-dependent error and the sensor-dependent error are coupled with the wiggling error in the experiments [17], and the effect of the wiggling random error is ignored during the compensation process. Therefore, the wrong compensation value often appears in the actual measurement, resulting in lower accuracy of ToF imaging systems.

It is more reasonable to directly avoid the wiggling-related error from its source, which means reducing its production instead of attempting to compensate for it. A number of studies have focused on reducing the wiggling-related error by optimizing the emitted signals of indirect ToF imaging systems. Hussmann et al. [18] developed an illumination module that emitted a sine-shape signal with no higher harmonics. Nevertheless, this method increased the random error of the system. Payne et al. [19] designed a harmonic rejection sampling methodology, which attenuated harmonics during the sampling. The experiments verified that this approach improved the measurement linearity in the raw measurements, but the random noise was also amplified. In [20,21], adjusting the duty cycle of the square wave signals proved to be effective in correcting the wiggling-related error. However, the ideal square wave is still challenging to produce, which limits this method.

The wiggling-related error can also be reduced by changing the calculation formula of the phase shift [22]. Drayton et al. [23] used the windowed discrete Fourier algorithm to calculate the phase shift. This algorithm had a good performance in reducing the error yet changed the sampling process of the ToF imaging system, which is difficult to implement in commercial indirect ToF imaging systems. The same problem also appeared in [15], where the third harmonic was eliminated with no significant increase in noise variance observed. In addition, Feigin et al. used the matrix pencil method to correct the wiggling error and the wiggling random error [24]. This method utilized the depth data of multiple frequencies and hence had low real-time performance.

All these methods suffer from technical disadvantages, such as an increase in the wiggling random error, difficulty to achieve, or low robustness. For this problem, we propose a wiggling-related error correction method for indirect ToF imaging systems. The main contributions include:

• To reduce the wiggling error, we propose a wiggling error cancelation method. This method adds a delay measurement without changing the hardware of ToF imaging systems, which is easy to implement in most indirect ToF imaging systems.
• To reduce the wiggling random error, we propose a wiggling random error reduction method based on an adaptive Kalman filter (AKF) for indirect ToF imaging systems. The method, which has good adaptive performance, utilizes the data of raw intensity measurements to evaluate the system state in real-time and calculates a more accurate phase.
• We establish a mathematical model of the wiggling-related error, which clearly shows the characteristics of the wiggling-related error. Combining (1) and (2), we propose a wiggling-related error correction method. The method is verified to have good performance and robustness in improving the range accuracy of indirect ToF imaging systems.

This article is organized as follows: Section 2 introduces the principle of ToF imaging systems and derives the mathematical model of the wiggling-related error. Section 3 describes the proposed wiggling-related error correction method in detail. Section 4 presents the simulation and experimental results and performs the discussions. Finally, the conclusions are presented in Section 5.

2. Principle

2.1. Principle of ToF Imaging Systems

Indirect ToF imaging systems use cross-correlation to indirectly determine the phase shift of the reflected signal [25]. The working principle of the indirect ToF imaging system is illustrated in Figure 2. The light source, triggered by an internal reference signal $s\left(t\right)$, is used to emit the modulated near infrared (NIR) light. After being reflected by the object, the light will be detected by the pixel field of the ToF sensor. On each pixel, the reflected light is converted to an electronic signal $r\left(t\right)$ and immediately correlates with the reference signal $s\left(t\right)$ at four phase shifts ${\theta }_n=n\pi /2$ with $n=0–3$, producing four intensity measurements $I_0$ to $I_3$ [13,26]. The process can be expressed as a correlation function:

$$I_n=I\left({\theta }_n\right)=r\left(t\right)\otimes s\left(t\right)=\frac{1}{T}\underset{-T/2}{\overset{T/2}{{\displaystyle \int }}}r\left(t\right)s\left(t-{\theta }_n\right)\mathrm{d}t=\frac{1}{T}\underset{-T/2}{\overset{T/2}{{\displaystyle \int }}}r\left(t\right)s\left(t-\frac{n\pi }{2}\right)\mathrm{d}t,n=0,1,2,3.$$

(1)

Both the reference and reflected signals are assumed to be sinusoidal functions:

$$s\left(t\right)=A_s\sin \left(2\pi ft\right)+B_s,$$

(2)

$$r\left(t\right)=A_r\sin \left(2\pi ft-\phi \right)+B_r,$$

(3)

where $f$ is the modulation frequency of ToF imaging systems, and $\phi$ is the phase shift relating to the round-trip time. $A_s$ and $A_r$ are the corresponding amplitudes of the reference and reflected signals.$B_s$ and $B_r$ are the corresponding offsets of the reference and reflected signals.

Substitute Equations (2) and (3) into Equation (1), the intensity measurements $I_0$ to $I_3$ are simplified to:

$$I_n=\frac{A_s{A}_r}{2}\cos \left(\phi -\frac{n\pi }{2}\right)+B_s{B}_r=A\cos \left(\phi -\frac{n\pi }{2}\right)+B,n=0,1,2,3,$$

(4)

where $A$ represents the amplitude of the correlation function, which is related to the sensitivity of the ToF sensor, the amplitude of the reference signal, the reflectivity of the scene, and the attenuation of the light. $B$ represents the offset of the correlation function, which is determined by the intensity of the background light [23].

With the four-step phase shifting algorithm [27], the phase and amplitude can be estimated as:

$$\phi =\arctan \left(\frac{I_1-I_3}{I_0-I_2}\right),$$

(5)

$$A=\frac{\sqrt{{\left(I_0-I_2\right)}^2+{\left(I_1-I_3\right)}^2}}{2}.$$

(6)

From the measured phase and the speed of light c, the range is finally inferred by:

$$d=\frac{\phi c}{4\pi f}.$$

(7)

2.2. Analysis of the Wiggling-Related Error

Due to the harmonics in the modulated signal, indirect ToF imaging systems suffer from the wiggling-related error, which significantly impacts the range accuracy. The wiggling-related error consists of the wiggling error and the wiggling random error. In this section, a mathematical model is derived to predict the wiggling error and the wiggling random error.

2.2.1. Wiggling Error

In the above, we assume that both the reference and reflected signals follow sinusoidal shapes. However, they are non-sinusoidal waveforms, causing higher-order harmonics in the correlation function. The higher-order odd harmonics cannot be filtered by the four-step phase shifting algorithm and thus will be aliased on the fundamental harmonic [28]. This leads to the wiggling error of the measured phase carried out with Equation (5). As illustrated in Figure 3a, there is a nonlinear relation between the measured phase and the true phase.

This paper considers odd harmonics up to the fifth-order, and the other higher harmonics can be ignored because of their insignificant amplitudes. According to [12], the intensity measurements of the correlation function with the third and fifth harmonics can be expressed as:

$$I_n^{\prime }=A_1\cos \left(\phi -\frac{n\pi }{2}\right)+A_3\cos \left[3\left(\phi -\frac{n\pi }{2}\right)\right]+A_5\cos \left[5\left(\phi -\frac{n\pi }{2}\right)\right]+B,n=0,1,2,3,$$

(8)

where $A_1$, $A_3$, and $A_5$ are the corresponding amplitudes of the fundamental, third-order, and fifth-order harmonics. Submitting Equation (8) into Equation (5), the measured phase can be written as:

$${\phi }^{\prime }=\arctan \left(\frac{I_1^{\prime }-I_3^{\prime }}{I_0^{\prime }-I_2^{\prime }}\right)=\arctan \left(\frac{A_1\sin \phi -A_3\sin 3\phi +A_5\sin 5\phi }{A_1\cos \phi +A_3\cos 3\phi +A_5\cos 5\phi }\right).$$

(9)

The wiggling error can be derived as the following form [29]:

$$\begin{array}{ll}\Delta \phi & ={\phi }^{\prime }-\phi =\arctan \left(\frac{A_1\sin \phi -A_3\sin 3\phi +A_5\sin 5\phi }{A_1\cos \phi +A_3\cos 3\phi +A_5\cos 5\phi }\right)-\arctan \left(\frac{\sin \phi }{\cos \phi }\right)\\ & =\arctan \left[\frac{-\left(A_3-A_5\right)\sin 4\phi }{A_1+\left(A_3+A_5\right)\cos 4\phi }\right]=-\arctan \left(\frac{q\sin 4\phi -r\sin 4\phi }{1+q\cos 4\phi +r\cos 4\phi }\right),\end{array}$$

(10)

where $q=A_3/A_1$ and $r=A_5/A_1$. Using the Taylor expansion, the above equation is simplified to:

$$\Delta \phi \approx -\left(q-r\right)\sin 4\phi +\left(\frac{q^2}{2}-\frac{r^2}{2}\right)\sin 8\phi .$$

(11)

Typically, we have $r\ll q\ll 1$, because $A_1$ is much larger than $A_3$ and $A_3$ is much larger than $A_5$. Therefore, the part of $\sin 8\phi$ in Equation (11) can be ignored. The wiggling error can be written in a more concise form:

$$\Delta \phi \approx -\left(q-r\right)\sin 4\phi =-p\sin 4\phi ,$$

(12)

where $p$ is a constant related to the harmonic component. Equation (12) indicates that the magnitude of the wiggling error depends on the true phase and the amplitudes of the harmonics. In the simulation, the wiggling error shows four sine periods as the true phase transverses from 0 to 2π (see Figure 3b). The maximum wiggling error reaches ±38.01 mrad, leading to a distance measurement error of ±75.62 mm under the modulation frequency of 12 MHz.

2.2.2. Wiggling Random Error

The main noise sources of indirect ToF imaging systems are photon shot noise, dark noise, and readout noise [30,31]. Among these noise sources, the shot noise is the dominant noise source in indirect ToF imaging systems [32]. This noise is introduced in the intensity measurement process, which can be regarded as counting photons by the charge and leads to a random error of phase measurement. In this paper, we focus on the random error caused by the photon shot noise. According to [12,33], we can approximate the intensity measurement as a random process with Gaussian distribution. Supposing the measured intensities $I_0$ to $I_3$ have the same variance ${\sigma }^2$, the variance of the measured phase can be estimated as [12]:

$${\sigma }_{\phi }^2=\frac{{\sigma }^2}{2A^2},$$

(13)

where $A$ is the measured amplitude calculated by Equation (6).

Normally, the variance ${\sigma }^2$ in ToF imaging systems can be considered to be an invariant constant, which is a safe assumption for ideal scenes since intensities are generated through the same process with the same intensity of light by the charges in the ToF sensor. However, the measured amplitude $A$ is phase-dependent when harmonics are present, as shown in Figure 4a. It can be deduced from Equation (13) that the variance ${\sigma }_{\phi }^2$ is therefore related to the true phase. As shown in Figure 4b, the magnitude of the variance also shows periodic oscillations, and the maximum variance of the measured phase is approximately 19.61 mrad². Therefore, the presence of harmonics and shot noise causes the wiggling random error of the measured phase.

3. Methods

3.1. Wiggling Error Cancelation Method

It can be seen from Figure 3b that the wiggling error of indirect ToF imaging systems is a sinusoidal periodic function. Taking advantage of the periodicity, we propose a wiggling error cancelation method (see Figure 5), which combines two phase measurements to eliminate the wiggling error.

As illustrated in Figure 5a, the phase shift of the reflected signal is measured without other operations in the first measurement. Theoretically, the true phase, the wiggling error, and the measured phase are as follows:

$${\phi }_{1\mathrm{st}}^{real}=\phi ,$$

(14)

$$\Delta {\phi }_{1\mathrm{st}}=-p\sin 4{\phi }_{1\mathrm{st}}^{real}=-p\sin 4\phi ,$$

(15)

$${\phi }_{1\mathrm{st}}^{measure}={\phi }_{1\mathrm{st}}^{real}+\Delta {\phi }_{1\mathrm{st}}=\phi -p\sin 4\phi ,$$

(16)

where $\phi$ is the phase shift corresponding to the physical distance $d$. In the second measurement, the emitted signal is delayed for T/8, which is equivalent to a phase delay of π/4. Therefore, the phase shift of the reflected signal will also increase by π/4. The true phase, the wiggling error, and the measured phase in the second measurement are as follows:

$${\phi }_{2\mathrm{nd}}^{real}=\phi +\pi /4,$$

(17)

$$\Delta {\phi }_{2\mathrm{nd}}=-p\sin 4{\phi }_{2\mathrm{nd}}^{real}=-p\sin 4\left(\phi +\pi /4\right)=p\sin 4\phi ,$$

(18)

$${\phi }_{2\mathrm{nd}}^{measure}={\phi }_{2\mathrm{nd}}^{real}+\Delta {\phi }_{2\mathrm{nd}}=\phi +\pi /4+p\sin 4\phi .$$

(19)

It can be inferred from Figure 5b, Equation (15), and Equation (18) that the wiggling errors of the two measurements are exactly opposite. Therefore, we add the measured phases of the two measurements to cancel the wiggling error and get the phase shift $\phi$:

$${\phi }_{1\mathrm{st}}^{measure}+{\phi }_{2\mathrm{nd}}^{measure}-\pi /4=2\phi .$$

(20)

The operation process of the wiggling error cancelation method is as follows:

• In the first measurement, calculate the measured phase ${\phi }_{1\mathrm{st}}^{measure}$ without other operations;
• In the second measurement, delay the emitted signal for T/8 and calculate the measured phase ${\phi }_{2\mathrm{nd}}^{measure}$;
• Combine ${\phi }_{1\mathrm{st}}^{measure}$ and ${\phi }_{2\mathrm{nd}}^{measure}$ to calculate the measured phased after the wiggling error cancelation:

  $${\phi }^{measure}=\frac{{\phi }_{1\mathrm{st}}^{measure}+{\phi }_{2\mathrm{nd}}^{measure}-\pi /4}{2}.$$

  (21)

3.2. Wiggling Random Error Reduction Method

The magnitude of the measured phase’s variance (or noise level) may be different at different true distances, so traditional filtering algorithms are difficult to achieve a good effect in denoising. For this reason, this section depicts a wiggling random error reduction method based on an adaptive Kalman filter for indirect ToF imaging systems.

First, we establish the discrete linear system model of ToF imaging systems [34]. The Kalman filter assumes that the real state of the system at time $k$ is evolved from the state at time $k-1$. The state-space equation and the observation equation are written as:

$$x_k=F{x}_{k-1}+w_k,$$

(22)

$$z_k=H{x}_k+v_k,$$

(23)

where $x_k$ is the system state vector, $F$ is the state transition matrix, and $w_k$ is the process noise with a Gaussian distribution, whose covariance matrix is $Q$. $z_k$ is the measurement vector, $H$ is the measurement matrix, $v_k$ is the measurement noise with a Gaussian distribution, whose covariance matrix is $R$.

For ToF imaging systems, the measurement vector $z_k$ is formed by the intensity measurements $I_0$ to $I_3$ at time $k$:

$$z_k={\left[\begin{array}{cccc}{I}_{0k}& I_{1k}& I_{2k}& I_{3k}\end{array}\right]}^T.$$

(24)

According to Equation (4), the intensity measurements at time $k$ can be written as:

$$\begin{array}{ll}{I}_{nk}& =A_k\cos \left({\phi }_k-\frac{n\pi }{2}\right)+B_k=A_k\left(\cos {\phi }_k\cos \frac{n\pi }{2}+\sin {\phi }_k\sin \frac{n\pi }{2}\right)+B_k\\ & =\left[\begin{array}{ccc}\cos \frac{n\pi }{2}& \sin \frac{n\pi }{2}& 1\end{array}\right]{\left[\begin{array}{ccc}{A}_k\cos {\phi }_k& A_k\sin {\phi }_k& B_k\end{array}\right]}^T,n=0,1,2,3.\end{array}$$

(25)

Submitting Equation (25) into Equation (24), the measurement vector can be further derived as:

$$z_k=\left[\begin{array}{c}{I}_{0k}\\ I_{1k}\\ I_{2k}\\ I_{3k}\end{array}\right]=\left[\begin{array}{ccc}\cos 0& \sin 0& 1\\ \cos \pi /2& \sin \pi /2& 1\\ \cos \pi & \sin \pi & 1\\ \cos 3\pi /2& \sin 3\pi /2& 1\end{array}\right]\left[\begin{array}{c}{A}_k\cos {\phi }_k\\ A_k\sin {\phi }_k\\ B_k\end{array}\right]=H{x}_k.$$

(26)

From the above description, we can obtain the system state vector $x_k$ and the measurement matrix $H$:

$$x_k={\left[\begin{array}{ccc}{A}_k\cos {\phi }_k& A_k\sin {\phi }_k& B_k\end{array}\right]}^T,$$

(27)

$$H=\left[\begin{array}{rrr}\hfill 1& \hfill 0& \hfill 1\\ \hfill 0& \hfill 1& \hfill 1\\ \hfill -1& \hfill 0& \hfill 1\\ \hfill 0& \hfill -1& \hfill 1\end{array}\right].$$

(28)

In a static scene, we have $x_k=x_{k-1}+w_k$. Thus, the state transition matrix $F$ can be expressed as:

$$F=\left[\begin{array}{rrr}\hfill 1& \hfill 0& \hfill 0\\ \hfill 0& \hfill 1& \hfill 0\\ \hfill 0& \hfill 0& \hfill 1\end{array}\right].$$

(29)

Therefore, indirect ToF imaging systems are linearized. From the system state vector shown in Equation (27), the measured phase at time $k$ can be estimated as:

$${\phi }_k=\arctan \left(\frac{x_k\left[2\right]}{x_k\left[1\right]}\right).$$

(30)

It can be noticed from Section 2.2 that the variance of the intensity measurement is constant, so the covariance matrix of the measurement noise $R$ can be regarded as a constant. However, due to the wiggling random error, the measured phase’s variance may differ at different true phases. It can be deduced that the level of process noise $w_k$ in Equation (22) varies with the phase. Therefore, the covariance matrix $Q$ is not a constant and varies with the true phase. The standard Kalman filter (SKF) consists of prediction and update [35], where $R$ and $Q$ are regarded as constants. Therefore, the SKF is hard to estimate the state of ToF imaging systems accurately. To address this issue, we introduce an evaluation stage based on the maximum likelihood criterion in the adaptive Kalman filter to adapt $Q$ [36]. The AKF can adjust the filter gain factor according to the covariance matrix $Q$, effectively reducing the temporal Gaussian noise and hence the wiggling random error. The specific steps of the AKF are given as follows:

• Initialization: Initialize the state estimate ${\widehat{x}}_0$, the error covariance matrix $P_0$, the covariance matrix $Q$, and the covariance matrix $R$;
• Time Prediction: Calculate the predicted state estimate ${\widehat{x}}_k^{-}$ and the predicted error covariance matrix $P_k^{-}$:

  $${\widehat{x}}_k^{-}=F{\widehat{x}}_{k-1},$$

  (31)

  $$P_k^{-}=F{P}_{k-1}{F}^T+Q;$$

  (32)
• Measurement Update: Calculate the Kalman gain $K_k$, the updated error covariance matrix $P_k$, the innovation sequence $r_k$, and the updated state estimate ${\widehat{x}}_k$:

  $$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R},$$

  (33)

  $$P_k=\left(I-K_{k}H\right)P_k^{-},$$

  (34)

  $$r_k=z_k-H{\widehat{x}}_k^{-},$$

  (35)

  $${\widehat{x}}_k={\widehat{x}}_k^{-}+K_k{r}_k,$$

  (36)

  where $I$ is a 3 × 3 identity matrix;
• Covariance Estimation: Calculate the estimated covariance of the innovation sequence ${\widehat{C}}_{rk}$, and update the covariance matrix $Q$:

  $${\widehat{C}}_{rk}=\frac{1}{L}{\displaystyle \sum }_{j=k-L+1}^k{r}_j{r}_j^T,$$

  (37)

  $$Q=K_k{\widehat{C}}_{rk}{K}_k^T,$$

  (38)

  where $L$ represents the length of the sliding window in the AKF.

The wiggling random error reduction method is illustrated in Figure 6. The method inputs the measurement vector $z_k$ (intensity measurements of a ToF imaging system) into the AKF, and outputs the system state vector $x_k$. Combining Equation (30), we can calculate the measured phase after the wiggling random error reduction.

3.3. Wiggling-Related Error Correction Method

Combining the contents of Section 3.1 and Section 3.2, we propose a wiggling-related error correction method, shown in Figure 7. The ToF imaging system performs two measurements successively and obtain the measurement vectors $z_{1\mathrm{st}}$ and $z_{2\mathrm{nd}}$. Then, the corrected phase is obtained by the following three steps:

• State Evaluation: Input $z_{1\mathrm{st}}$ and $z_{2\mathrm{nd}}$ into the AKF and obtain the system state vectors $x_{1\mathrm{st}}$ and $x_{2\mathrm{nd}}$;
• Phase Calculation: Calculate the measured phases ${\phi }_{1\mathrm{st}}^{measure}$ and ${\phi }_{2\mathrm{nd}}^{measure}$ for the two measurements. In this step, the wiggling random errors of two measurements are reduced;
• Phase Correction: Calculate the corrected phase ${\phi }^{measure}$. In this step, the wiggling error is reduced.

4. Results and Discussions

In this section, we first verify the performance of the wiggling-related error correction method by MATLAB simulation, where the intensity measurements are constructed according to the model in Section 2.2. Then, using the experimental platform built in Section 4.2, the derived mathematical model and effectiveness of the proposed method applied on real measurements are verified.

4.1. Simulation

In the simulation, two measurements were implemented at different true phases from 0 to 2π with steps of π/180. At each true phase, 2000 frames of data for the first and second measurements were simulated separately. The measured intensities of two measurements at the $i$th true phase in the $j$th frame were constructed with Equations (39) and (40), where ${\phi }_i$ is the true phase, and $N\left(j\right)$ represents the Gaussian noise with a mean value of zero and a variance of ${\sigma }^2$.

Table 1. The parameters used in the simulation.

Parameters	Symbol	Value
The amplitude of the fundamental harmonic	$A_1$	500 LSB
The amplitude of the third-order harmonic	$A_3$	20 LSB
The amplitude of the fifth-order harmonic	$A_5$	1 LSB
The offset of the correlation function	$B$	500 LSB
The variance of the intensity measurement	${\sigma }^2$	9 LSB2
The initial state estimate	${\widehat{x}}_0$	${\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$
The initial error covariance matrix	$P_0$	$I_{3\times 3}$
The initial covariance matrix of the process noise	$Q$	$0.5I_{3\times 3}$
The initial covariance matrix of the measurement noise	$R$	$10I_{4\times 4}$
The length of the sliding window	$L$	20

lists the parameters used in the simulation.

$$I_{n,i}^{1\mathrm{st}}\left(j\right)=A_1\cos \left({\phi }_i-\frac{n\pi }{2}\right)+A_3\cos \left[3\left({\phi }_i-\frac{n\pi }{2}\right)\right]+A_5\cos \left[5\left({\phi }_i-\frac{n\pi }{2}\right)\right]+B+N\left(j\right),n=0,1,2,3.$$

(39)

$$I_{n,i}^{2\mathrm{nd}}\left(j\right)=A_1\cos \left[\left({\phi }_i+\frac{\pi }{4}\right)-\frac{n\pi }{2}\right]+A_3\cos \left[3\left[\left({\phi }_i+\frac{\pi }{4}\right)-\frac{n\pi }{2}\right]\right]+A_5\cos \left[5\left[\left({\phi }_i+\frac{\pi }{4}\right)-\frac{n\pi }{2}\right]\right]+B+N\left(j\right),n=0,1,2,3.$$

(40)

For the measured phases, we applied the standard deviation (STD) to evaluate the wiggling random error. The Root-mean-square error (RMSE) was chosen to evaluate the wiggling-related error in general. Both the systematic error (wiggling error) and the random error affect the magnitude of the RMSE. The STD and the RMSE of the measured phase at the $i$th true phase are expressed as:

$$ST{D}_i=\sqrt{\frac{1}{N}{\displaystyle \sum }_{j=1}^N{\left[{\phi }_i^M\left(j\right)-\overline{{\phi }_i^M}\right]}^2},$$

(41)

$$RMS{E}_i=\sqrt{\frac{1}{N}{\displaystyle \sum }_{j=1}^N{\left[{\phi }_i^M\left(j\right)-{\phi }_i\right]}^2},$$

(42)

where $N$ represents the number of measured frames at each phase, ${\phi }_i^M\left(j\right)$ is the measured phase at the $i$th true phase in the $j$th frame, $\overline{{\phi }_i^M}$ is the average of the measured phases at the $i$th true phase.

We first evaluated the denoising performance of the AKF. Figure 8a–c show the measured phase sequence at six of the true phases. In each figure, the true phase sequence was kept constant. We inputted the measurement vector sequences of the first measurement at different true phases into SKF or AKF and obtained the measured phase sequences. The raw measured phase sequences were obtained by Equation (5). It was obvious that AKF had better performance than SKF and effectively reduced the STD of the measured phase on the selected true phases, thereby reducing the wiggling random error. In addition, it can be found that there is a deviation between the measured phase and the true phase (see Figure 8a,c,d,f), which was caused by the wiggling error.

Furthermore, we evaluated the performance of the wiggling-related error correction method (see Figure 9). The measured phase before correction was calculated by Equation (5), using the first measurement. Using the two measurements shown in Equations (39) and (40), we obtained the corrected phase according to the method in Section 3.3. The phase error at each frame is plotted as the points in Figure 9a. The blue and red lines connect the average errors at different true phases before and after correction, representing the wiggling errors. Figure 9b,c show the STD and the RMSE of the measured phase, respectively, where the phase before correction, for fairness, uses double the first measurement data (4000 frames).

The result, as shown in Figure 9a, shows a four-cycle wiggling error before correction. The peak-to-peak value (PPV) of the wiggling error was approximately 76.14 mrad. In contrast, the wiggling error significantly reduced after correction, and the PPV of the wiggling error reduced to about 1.83 mrad. The remaining wiggling error was caused by the part of $\sin 8\phi$ in Equation (11). Moreover, there was a significant random error before correction. As we can see, the blue dots were distributed around the blue line. It can be noticed from the distribution of the red dots and Figure 9b that the random error greatly reduced. The arithmetic mean of STDs ${\mu }_{STD}$ over all phases decreased from 4.24 to 0.28 mrad. In general, the proposed method performed well in the wiggling-related error correction. As illustrated in Figure 9c, the arithmetic mean of RMSEs ${\mu }_{RMSE}$ over all phases reduced significantly from 24.81 to 0.60 mrad, which means that the wiggling-related error was only 2.4% of that before correction.

4.2. Experiment

The experimental system is shown in Figure 10. We used a high-reflectivity plate as the measured object to avoid the amplitude-dependent error caused by non-linearities of the ToF sensor [17,37,38]. The ToF imaging system was attached to the slide rail. In the ToF imaging system, the delay generator of the ToF sensor could delay the emitted light signal by T/8 without changing the hardware, thereby realizing the wiggling error cancelation. The phase calculation and the phase correction were performed in the processing module of the ToF imaging system, where the parameters of AKF were the same as those set in the simulation.

To obtain the phase error for different true phases, we typically moved the plate to different distances. However, the wiggling-related error cannot be accurately presented by using this approach. On the one hand, the measured intensity is saturated at close distances, leading to a phase calculation bias. On the other hand, the amplitude of the reflected signal significantly decreases at long distances, which causes an amplitude-dependent error. A more sensible way is to add an electronic delay to the emitted signal. In this way, different distances are emulated by changing the delay. This approach minimizes the effect of the other errors on the experiment, such as the amplitude-dependent error, the multipath effect of the scene, and the non-uniformity error [39].

In the experiment, the modulation frequency of the ToF imaging system was set to 12 MHz. Therefore, the unambiguous distance was 12.5 m according to Equation (7). The physical distance between the plate and the ToF imaging system remained unchanged. Then, the electronic delay increased in steps of 2 ns, equivalent to an increase of 0.048π in the true phase, until the true phase spans 0 to 2π (0–12.5 m of the true distance). At each true phase, 200 frames of the data were captured. The experiment was carried out under the integration times of 600 μs, 400 μs, and 200 μs. The results are shown in Figure 11, Figure 12 and Figure 13.

Table 2. Statistical result of Figure 11, Figure 12 and Figure 13.

Parameter	600 μs		400 μs		200 μs
	Before	After	Before	After	Before	After
PPV (mrad)	89.15	11.85	88.70	12.44	89.90	12.75
${\mu }_{STD}$ (mrad)	4.35	0.51	4.98	0.58	6.72	0.71
${\mu }_{RMSE}$ (mrad)	28.03	2.20	28.85	2.44	28.95	3.04

gives the statistical parameters, including the PPV of wiggling error in Figure 11, the average of STDs ${\mu }_{STD}$ in Figure 12 and the average of RMSEs ${\mu }_{RMSE}$ in Figure 13.

Figure 11 shows the phase error in the experiment. As expected, we can notice four oscillations with the true phase spanning 0 to 2π before correction. The PPVs of the wiggling errors at integration times of 600 μs,400 μs, and 200 μs were approximately 89.15, 88.70, and 89.90 mrad. This error was reduced to the PPVs of 11.85, 12.24, and 12.75 mrad after correction, which was a significant improvement.

As shown in Figure 12, the STD, which represents the magnitude of the wiggling random error, also demonstrated the predicted wiggling phenomenon. It can be noticed from Table 2 that the random error increased as the integration time decreased. At integration times of 600 μs, 400 μs, and 200 μs, values of ${\mu }_{STD}$ are 4.35, 4.98, and 6.72 mrad, respectively. The proposed method had good adaptive performance and performed well in the wiggling random error reduction at various integration times. The values of ${\mu }_{STD}$ were about 0.51, 0.58, and 0.71 mrad after correction.

Figure 13 shows the RMSE of the measured phase for different true phases. We compared the ${\mu }_{RMSE}$ in Table 2, which represents the magnitude of the wiggling-related, and found that values of ${\mu }_{RMSE}$ were reduced by about 92.2% (integration time of 600 μs), 91.5% (integration time of 400 μs), and 89.5% (integration time of 200 μs). Therefore, the proposed method can effectively improve the range accuracy of the indirect ToF imaging system and has certain robustness in different integration times.

It can be noticed from the experimental results that the wiggling-related error followed the shape of the simulation. However, the values of each parameter obtained in the experiment were larger than those in the simulation. This phenomenon was because the harmonic component of the emitted signal in the experiment was more complicated than the one in the simulation, and temperature also affected the results. Despite all this, experimental results can still verify the derived mathematical model. In addition, the performance of the proposed method degrades a little as the integration time decreases, which is caused by the PMD sensor’s nonlinearity.

To better illustrate the effectiveness of the proposed method, we compared the method with several existing methods in [15,18,20]. The PPV of the phase error, calculated as the difference between the maximum and minimum phase errors, was chosen to evaluate the wiggling-related error before and after correction. The result is shown in

Table 3. Comparison of the proposed method with the existing methods. The phase error in [15] is obtained directly from the results of the paper, while the phase errors in [18,20] are calculated from the distance error and the modulation frequency in the corresponding documents. The data of the proposed method is calculated from the phase error under the integration time of 600 μs.

Method	Harmonics Cancellation	PPV of the Phase Error (mrad)		Reduction Ratio	Implementation
		Before	After
Streeter et al. [15]	3rd	70.00	20.00	71.4%	Change intensity measurement process
Hussmann et al. [18]	2nd–5th	83.78	11.73	86.0%	Add an illumination module
Payne et al. [20]	3rd and 5th	99.53	19.35	80.6%	Add an additional FPGA
The proposed method	3rd and 5th	114.50	14.64	87.2%	Add a delay measurement

. Compared with the existing methods, the proposed method has the optimal performance in correcting the wiggling-related error. In addition, the existing methods require some hardware changes, while our method is easier to implement in most indirect ToF imaging systems.

5. Conclusions

In this paper, we derive a mathematical model of the wiggling-related error and propose a wiggling-related error correction method for indirect ToF imaging systems. This method adds a delay measurement and utilizes raw intensity measurements to evaluate the system state based on the AKF, which is easy to implement in most indirect ToF imaging systems. Simulation and experimental results showed that the wiggling-related error is significant, and the proposed method has a good performance in reducing the wiggling-related error. The PPV of the wiggling error significantly reduced after correction (from 89.15 to 11.85 mrad at the integration time of 600 μs). Meanwhile, ${\mu }_{STD}$ decreased by at least 88.3% (from 4.35 to 0.51 mrad at the integration time of 600 μs). Moreover, the proposed method had good robustness under different integration times. The values of ${\mu }_{RMSE}$ reduced by about 92.2% (integration time of 600 μs), 91.5% (integration time of 400 μs), and 89.5% (integration time of 200 μs).

We believe that this study provides effective guidance for researchers understanding the wiggling-related error and a potential direction for the accuracy improvement of indirect ToF imaging systems. Our future works will further optimize the proposed method to develop ToF imaging systems with higher performance.