Systematic Error Modeling and Bias Estimation

Abstract

This paper analyzes the statistic properties of the systematic error in terms of range and bearing during the transformation process. Furthermore, we rely on a weighted nonlinear least square method to calculate the biases based on the proposed models. The results show the high performance of the proposed approach for error modeling and bias estimation.

1. Introduction

Interest in Cooperative Localization (C.L.) has grown exponentially in the past few years [1,2]. The concept of C.L. is demonstrated by collecting a large amount of data from heterogeneous sensors to improve the volume of surveillance and increase the estimation reliability [3]. This requires local sensors transforming their data to a common reference system for further processing. Due to the nonlinear issues, the transformations often introduce systematic errors [4,5]. Hence, the sensor registration algorithms are quite important.

Sensor registration mainly refers to the systematic errors, in contrast to the random errors. Various methods have been proposed including the centralized [6,7,8] and decentralized solutions [9,10,11,12]. In the centralized solution, the exact maximum likelihood method is proposed where the sensor measurements were first projected onto the local coordinate, and then transformed to the public region [13]. However, the errors introduced from stereographic projection suffer in both the local sensor and the public region. Regarding the decentralized solution, Lin et al. presented the bias estimation based on the local tracks at different frames [14]. This work considered both offset biases and scale biases and demonstrated some preliminary results. However, this technique estimates the biases during the filtering phases, which suffer from target maneuvers. Zhang et al. presented the bias estimation based on the probability hypothesis density filter, in which only the translational biases were considered [15].

In this paper, the statistic properties of the systematic errors are first analyzed, with respect to the mean and covariance. Then the weighted nonlinear least square method is utilized to estimate the corresponding biases in terms of range and bearing. The proposed solution also considers the maneuvers by normalizing the weights of the pseudo-measurements based on the covariances.

This paper is organized as follows: Section 2 investigates the statistic proprieties of the systematic errors. Section 3 introduces the nonlinear least square method to estimate the biases. Section 4 presents simulation results in synchronous sensors. Finally, the paper is concluded in Section 5.

2. Analysis of Systematic Error Properties

In this section, the systematic error is analyzed with respect to the expectation and covariance in terms of range and bearing.

2.1. Problem Statement

In cooperative localization, measurements are collected in a polar coordinate system which also contains both biases and random noises. The measured range $r_m$ and bearing ${\theta }_m$ are thus defined as

$$r_m=\overline{r}+r_b+\tilde{r}$$

(1)

$${\theta }_m=\overline{\theta }+{\theta }_b+\tilde{\theta }$$

(2)

where $\overline{r}$ and $\overline{\theta }$ denote the ground truth, $r_b$ and ${\theta }_b$ denote the corresponding biases. $\tilde{r}$ and $\tilde{\theta }$ are assumed to be the Gaussian noises with zero mean and standard deviations ${\sigma }_r$ and ${\sigma }_{\theta }$. For the localization task, measurements from polar coordinates are transformed to Cartesian coordinates by using

$$x_m=r_{m}cos{\theta }_m$$

(3)

$$y_m=r_{m}sin{\theta }_m$$

(4)

2.2. True Systematic Error

The transformed measurement can also be represented as the combination of the true value and the systematic error.

$$x_m=r_{m}cos{\theta }_m=(\overline{r}+r_b+\tilde{r})cos(\overline{\theta }+{\theta }_b+\tilde{\theta })=\overline{x}+\tilde{x}$$

(5)

$$y_m=r_{m}sin{\theta }_m=(\overline{r}+r_b+\tilde{r})sin(\overline{\theta }+{\theta }_b+\tilde{\theta })=\overline{y}+\tilde{y}$$

(6)

where $\overline{x}=\overline{r}cos\overline{\theta }$ and $\overline{y}=\overline{r}sin\overline{\theta }$ denote the true values and $\tilde{x}$ and $\tilde{y}$ denote the systematic errors.

Rearranging Equtions (5) and (6), we have the representation of the systematic errors. Here we only show the systematic in x direction for simplification reasons.

$$\tilde{x}=r_{m}cos{\theta }_m-\overline{r}cos\overline{\theta }=\overline{r}[\mathbf{A}-cos\overline{\theta }]+r_b\left[\mathbf{A}\right]+\tilde{r}\left[\mathbf{A}\right]$$

(7)

and

$${\tilde{x}}^2={\overline{r}}^2{[\mathbf{A}-cos\overline{\theta }]}^2+r_b^2{\left[\mathbf{A}\right]}^2+{\tilde{r}}^2{\left[\mathbf{A}\right]}^2+2\overline{r}{r}_b[\mathbf{A}-cos\overline{\theta }]\left[\mathbf{A}\right]+2\overline{r}\tilde{r}[\mathbf{A}-cos\overline{\theta }]\left[\mathbf{A}\right]+2\tilde{r}{r}_b{\left[\mathbf{A}\right]}^2$$

(8)

where

$$\mathbf{A}=cos\overline{\theta }cos{\theta }_{b}cos\tilde{\theta }-sin\overline{\theta }sin{\theta }_{b}cos\tilde{\theta }-sin\overline{\theta }cos{\theta }_{b}sin\tilde{\theta }-cos\overline{\theta }sin{\theta }_{b}sin\tilde{\theta }$$

(9)

Instead of calculating systematic error by using ground-truths, biases and noises, the statistics properties are considered where the random noises are eliminated.

The expectation and covariance of the systematic error can be explicitly calculated with following equations:

$$E[cos\tilde{\theta }]=e^{-\frac{{\sigma }_{\theta }^2}{2}}$$

(10)

$$E[sin\tilde{\theta }]=0$$

(11)

$$E[{cos}^2\tilde{\theta }]=\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}$$

(12)

$$E[{sin}^2\tilde{\theta }]=\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}$$

(13)

$$E[sin\tilde{\theta }cos\tilde{\theta }]=0$$

(14)

The statistic properties of the systematic error are summarized as

$$E\left[\tilde{x}\right]=\overline{r}[cos\overline{\theta }cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}-sin\overline{\theta }sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}-cos\overline{\theta }]+r_b[cos\overline{\theta }cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}-sin\overline{\theta }sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}]$$

(15)

and

$$\begin{array}{c}\hfill \begin{array}{c}\hfill E\left[{\tilde{x}}^2\right]={\overline{r}}^2[\mathbf{B}+{cos}^2\overline{\theta }-2{cos}^2\overline{\theta }cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}+sin2\overline{\theta }sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}]+r_b^2\mathbf{B}+{\sigma }_r^2\mathbf{B}\\ \hfill +2\overline{r}{r}_b[\mathbf{B}-{cos}^2\overline{\theta }cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}+sin2\overline{\theta }sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}/2]\end{array}\end{array}$$

(16)

where $\mathbf{B}$ equals

$$\begin{array}{c}\hfill \mathbf{B}={cos}^2\overline{\theta }{cos}^2{\theta }_b(1+e^{-2{\sigma }_{\theta }^2})/2+{sin}^2\overline{\theta }{sin}^2{\theta }_b(1+e^{-2{\sigma }_{\theta }^2})/2+\\ \hfill {sin}^2\overline{\theta }{cos}^2{\theta }_b(1-e^{-2{\sigma }_{\theta }^2})/2+{cos}^2\overline{\theta }{sin}^2{\theta }_b(1-e^{-2{\sigma }_{\theta }^2})/2-sin2\overline{\theta }sin2{\theta }_b{e}^{-2{\sigma }_{\theta }^2}/2\end{array}$$

Similarly, the corresponding systematic error in y direction is calculated. Equation (15) and Equation (16) still depend on the ground-truths, which are unavailable in practice. Hence, the systematic errors are calculated again on condition of the measurements.

2.3. Systematic Error in Practice

Based on the Equation (15) and Equation (16), the conditional first and second order moments are calculated as

$$\begin{array}{c}\hfill E\left[E\left[\tilde{x}\right]\right|r_m,{\theta }_m]=r_m[cos({\theta }_m-{\theta }_b)cos{\theta }_b{e}^{-{\sigma }_{\theta }^2}-sin({\theta }_m-{\theta }_b)sin{\theta }_b{e}^{-{\sigma }_{\theta }^2}-\\ \hfill cos({\theta }_m-{\theta }_b)e^{-{\sigma }_{\theta }^2/2}]+r_{b}cos({\theta }_m-{\theta }_b)e^{-{\sigma }_{\theta }^2/2}\end{array}$$

(17)

$$\begin{array}{c}\hfill E\left[E\left[{\tilde{x}}^2\right]\right|r_m,{\theta }_m]=\mathbf{C}\times [\mathbf{M}+\mathbf{D}-2\mathbf{D}cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}+\mathbf{F}sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}]+r_b^2\left[\mathbf{M}\right]+{\sigma }_r^2\left[M\right]\\ \hfill +(2r_b{r}_m-2r_b^2)[\mathbf{M}-\mathbf{D}cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}+\frac{1}{2}\mathbf{F}sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}]\end{array}$$

(18)

and

$$\begin{array}{c}\hfill E\left[E\left[\tilde{y}\right]\right|r_m,{\theta }_m]=r_m[sin({\theta }_m-{\theta }_b)cos{\theta }_b{e}^{-{\sigma }_{\theta }^2}+cos({\theta }_m-{\theta }_b)sin{\theta }_b{e}^{-{\sigma }_{\theta }^2}-\\ \hfill sin({\theta }_m-{\theta }_b)e^{-{\sigma }_{\theta }^2/2}]+r_{b}sin({\theta }_m-{\theta }_b)e^{-{\sigma }_{\theta }^2/2}\end{array}$$

(19)

$$\begin{array}{c}\hfill E\left[E\left[{\tilde{y}}^2\right]\right|r_m,{\theta }_m]=\mathbf{C}\times [\mathbf{N}+\mathbf{E}-2\mathbf{E}cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}-\mathbf{F}sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}]+r_b^2\left[\mathbf{N}\right]+{\sigma }_r^2\left[\mathbf{N}\right]\\ \hfill +(2r_b{r}_m-2r_b^2)[\mathbf{N}-\mathbf{E}cos{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}-\frac{1}{2}\mathbf{F}sin{\theta }_b{e}^{-{\sigma }_{\theta }^2/2}]\end{array}$$

(20)

where

$$\mathbf{C}=r_m^2+r_b^2+{\sigma }_r^2-2r_m{r}_b$$

$$\mathbf{D}={cos}^2({\theta }_m-{\theta }_b)\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}+{sin}^2({\theta }_m-{\theta }_b)\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}$$

$$\mathbf{E}={sin}^2({\theta }_m-{\theta }_b)\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}+{cos}^2({\theta }_m-{\theta }_b)\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}$$

$$\mathbf{F}=sin2({\theta }_m-{\theta }_b)e^{-2{\sigma }_{\theta }^2}$$

$$\mathbf{M}=\mathbf{D}{cos}^2{\theta }_b\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{E}{sin}^2{\theta }_b\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{E}{cos}^2{\theta }_b\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{D}{sin}^2{\theta }_b\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}-\mathbf{F}sin2{\theta }_b\frac{e^{-2{\sigma }_{\theta }^2}}{2}$$

$$\mathbf{N}=\mathbf{E}{cos}^2{\theta }_b\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{D}{sin}^2{\theta }_b\frac{1+e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{D}{cos}^2{\theta }_b\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{E}{sin}^2{\theta }_b\frac{1-e^{-2{\sigma }_{\theta }^2}}{2}+\mathbf{F}sin2{\theta }_b\frac{e^{-2{\sigma }_{\theta }^2}}{2}$$

Equations (17)–(20) successfully estimate the systematic error by using the original measurements. The uncertainty of the estimation is also calculated as

$$var\left(\tilde{x}\right)=E\left[E\left[{\tilde{x}}^2\right]\right|r_m,{\theta }_m]-E^2\left[E\left[\tilde{x}\right]\right|r_m,{\theta }_m]$$

(21)

$$var\left(\tilde{y}\right)=E\left[E\left[{\tilde{y}}^2\right]\right|r_m,{\theta }_m]-E^2\left[E\left[\tilde{y}\right]\right|r_m,{\theta }_m]$$

(22)

3. Bias Estimation Using Nonlinear Least Square Method

To calculate the biases, information from additional sensors are required. Suppose both the radar and GPS measurements (GPS only contains random noises) are provided, we have

$$x_m^r-x_m^g={\tilde{x}}^r-{\tilde{x}}^g$$

(23)

$$y_m^r-y_m^g={\tilde{y}}^r-{\tilde{y}}^g$$

(24)

where $x_m^r$ and $y_m^r$ denote the transformed measurement, $x_m^g$ and $y_m^g$ denote the GPS measurement.

Based on the statistic properties of the systematic error, the expectations of Equations (23) and (24) are described as

$$x_m^r-x_m^g=E\left[E\left[\tilde{x}\right]\right|r_m,{\theta }_m]$$

(25)

$$y_m^r-y_m^g=E\left[E\left[\tilde{y}\right]\right|r_m,{\theta }_m]$$

(26)

To estimate the biases, a weighted nonlinear least square method is utilized in which the calculation formula is written as

$$\left|\right|e^2\left(\mathbf{x}\right)\left|\right|=\frac{1}{2}{e}^T\mathbf{W}e=\frac{1}{2}\sum _{i=1}^n{w}_i{[{\mathbf{y}}_i-{\mathbf{f}}_i\left(\mathbf{x}\right)]}^2$$

(27)

where

$$\mathbf{y}=\left[\begin{array}{c}{x}_m^r-x_m^g\\ y_m^r-y_m^g\end{array}\right],\mathbf{f}=\left[\begin{array}{c}E\left[E\left[\tilde{x}\right]\right|r_m{\theta }_m]\\ E\left[E\left[\tilde{y}\right]\right|r_m,{\theta }_m]\end{array}\right]$$

and $i=1,...,n$ is the index of the measurement, $\mathbf{W}$ is the weight matrix and calculated by the inverse of the deviation.

To calculate the bias $\mathbf{x}=(r_b,{\theta }_b)$, the gradient $▿\left|\right|e^2\left|\right|$ is utilized by

$$\frac{\partial \left|\right|e^2\left(\mathbf{x}\right)\left|\right|}{\partial \mathbf{x}}=-\sum _{i=1}^n{w}_i[{\mathbf{y}}_i-{\mathbf{f}}_i\left(\mathbf{x}\right)]\frac{\partial {\mathbf{f}}_i\left(\mathbf{x}\right)}{\partial \mathbf{x}}$$

(28)

Hence the bias is estimated with an initial value ${\mathbf{x}}_0$ by taking in the direction which $\left|\right|e^2\left|\right|$ drops most rapidly. More details of the nonlinear least square method could be found in [16].

4. Simulation

In this section, simulated data were used to evaluate the effectiveness of the proposed approach. The statistic properties of the systematic error are first evaluated by conducting 1000 times Monte-Carlo simulation. During the simulation, three objects are measured in 50 steps: (1) stationary object; (2) object with constant velocity; (3) object with maneuver (constant turn). The trajectories of all objects are exhibited in Figure 1. The radar is at the original point and the biases are given by $r_b$ = 15 m and ${\theta }_b$ = 0.3 rad, where the corresponding random noises are given by zero mean with deviation 1 m in range and ${0.1}^{\circ }$ in bearing.

Figure 2, Figure 3 and Figure 4 illustrate the performance of the systematic error analyzation for all objects by using the proposed models. Since the first object is a non-moving object, both the estimated expectation and covariance of the systematic error are close to constant values. The estimation for constant velocity and maneuver objects are exhibited in Figure 3 and Figure 4. It is observed that the errors are independent with the positions. It is also observed that both the expectation and covariance are significantly decreased when objects are close to the radar.

The biases are also estimated by using the non-linear least square estimator. During the estimation phase, additional information from GPS are required (zero mean, standard deviation 10 m). Note that the sensor alignment method [6] (normal least square method, using first order Taylor extension for linearizion) is also utilized to compare the performances.

Figure 5, Figure 6 and Figure 7 demonstrate the performance of both approaches, where the biases are recursively estimated in terms of range and bearing. Since the proposed approach operates on the raw data level, the estimation contains huge differences at beginning. With increasing time, the estimator successfully converges to the ground truths. It is observed that the proposed approach has high performance in all scenarios compared to the state-of-the-art.

To evaluate the performance quantitatively, the root mean square equation (RMSE) is also used as follows:

$$\text{RangeRMSE}=\sqrt{\frac{{\sum }_{i=1}^n{(\text{EstimatedRangeBias}-\text{TrueRangeBias})}^2}{n}}$$

(29)

$$\text{BearingRMSE}=\sqrt{\frac{{\sum }_{i=1}^n{(\text{EstimatedBearingBias}-\text{TrueBearingBias})}^2}{n}}$$

(30)

where n is the step index. Figure 8, Figure 9 and Figure 10 show the performance of both approaches evaluated by RMSE. Since the bearing bias is much smaller compared to the range bias, the corresponding RMSE is smaller in contrast to the range bias. Based on the calculation, it is concluded the overall performance of the proposed approach is better than the normal least square method.

Contributions are summarized as follows: First, the statistic properties of the systematic error are analyzed and evaluated. Second, a least square method is proposed to estimate sensor biases based on the proposed statistics properties. The benefit of the proposed systematic error model is that the unbiased measurement may be calculated during the filtering phase combined with the uncertainties. Furthermore, the sensor biases could also be calculated by solely relying on the measurements.

5. Conclusions

In this paper, the systematic error is analyzed and modeled with respect to its statistic properties. The proposed model not only calculates the expectation of the systematic error, but also gives the covariance. Furthermore, a nonlinear least square method is proposed to estimate sensor biases. In comparison to the related work, the proposed approach recursively estimates both the error and sensor biases in absence of the ground truths. The performance is evaluated by using 1000 times Monte-Carlo simulation and three objects with different maneuvers. A comparative study has also been carried out and exhibits the high performance of the proposed approach. Future work focuses on the application of the proposed approach in real scenarios.