3. Fundamentals 27
3.7. Cramér-Rao Lower Bound and Fisher Information
as
yt=φyt−1+t, (3.95)
where we dropped the subscript on the autocorrelation coefficientφ1.
Testing whether given data contains autocorrelated errors of order p = 1can be ac-complished by different test statistics, e.g., the Durbin-Watson test (Durbin and Watson, 1950). Letet be the residual for thet-th data point. The Durbin-Watson test statisticd is then
d= PT
t=2(et−et−1)2 PT
t=1e2t , (3.96)
whereT is the number of data points. The value ofd isd ≈ 2(1−φ)and lies within the interval [0,4]. It is compared to reference values to decide whether the errors are autocorrelated of order1. It is close tod ≈2forφ= 0.
The autocorrelation coefficientsφi can be computed in multiple ways. These include the Yule-Walker equations (Yule, 1927), maximum likelihood estimation, or the Burg method (Brockwell et al., 2005). For our needs we content ourselves with inspect-ing the partial autocorrelation function ofyt and determining the model order and the autocorrelation coefficient.
Within a NLLSQ framework autocorrelated measurements can be treated with gen-eralized least squares orestimation in first differences. The fields of statistics, econo-metrics, and financial mathematics have developed sophisticated tool sets for this, see for example the introduction by Maddala and Lahiri (1992). For our use case we are interested in time series of measurements whose noise is defined by an AR(1) model.
We describe in Section 6.4 a method to incorporate pose estimates with autocorrelated errors.
3.7. Cramér-Rao Lower Bound and Fisher Information
In this section we present the concepts of the Cramér-Rao Lower Bound (CRLB) and the Fisher Information (FI). They will prove useful when computing the covariance of our estimated solution in the case of AR(1) models in Section 6.4.3. This derivation
follows the Sections 2.1.8 and 2.2.11 in (Barfoot, 2017).
Assume that we want to estimate an unknown parameterθ by observing a random variableX. The FI quantifies the information that a random variableX carries about a parameterθof a PDF that modelsX. We can express this relationship as the conditional PDFp(X|θ). The FI allows us to make statements about the best possible parameter estimation within this model. The FI matrix for an N-dimensional parameter space is defined as
I(X|θ) = E
"
∂lnp(X|θ)
∂θ
>
∂lnp(X|θ)
∂θ
#
∈RN×N. (3.97) Suppose that we have obtained a set of realizationsx of the random variable X. We can think of these realizations as a set of measurements. The CRLB states that the uncertainty of any unbiased estimateθˆthat is based onxhas a lower bound. That is, given our measured information we cannot get more certain about our estimate than this lower bound. Now, the CRLB even allows us to compute this lower bound by stating that
cov( ˆθ|x)−I−1(X|θ)≥0. (3.98) Here the expressionA ≥ 0means that the matrixAis positive semi-definite7. There-fore, the CRLB fundamentally limits our certainty about the estimate of a parameter given a set of measurements. In other words, the measurements carry a limited amount of information for estimating a parameter. The FI quantifies this information.
3.7.1. Cramér-Rao Lower Bound applied to measurements from independent Gaussians
So far we have presented the relationship between the CRLB and the FI for general PDFs. We will now apply these concepts to measurements from statistically indepen-dent Gaussian PDFs.
Suppose we are interested in estimating the meanµof an N-dimensional Gaussian PDF. To this end, we obtain n measurementsxi ∈ RN of this distribution. The
loga-7IfAexpresses the difference between the estimated and the true covariance matrix, then this property is also calledcovariance consistency.
3.7. Cramér-Rao Lower Bound and Fisher Information 71 rithm of the joint probability of all measurements is
lnp(X|µ,Σ) = −1
2(x−Aµ)>B−1(x−Aµ) +const, (3.99) wherex = [x>1, . . . ,x>n]>,A = [1, . . . ,1]>
| {z }
nblocks
, and B = diag(Σ, . . . ,Σ)
| {z }
nblocks
. For comput-ing the CRLB we are interested in the derivative of (3.99) with respect to the estimated parameterµ:
∂lnp(X|µ,Σ)
∂µ = (x−Aµ)>B−1A. (3.100)
The FI matrix is therefore I(X|µ) =E
"
∂lnp(X|µ)
∂µ
>
∂lnp(X|µ)
∂µ
#
(3.101)
=E
A>B−1(x−Aµ)(x−Aµ)>B−1A
(3.102)
=A>B−1E
(x−Aµ)(x−Aµ)>
| {z }
=B
B−1A (3.103)
=A>B−1A (3.104)
=nΣ−1. (3.105)
Inserting this result into (3.98) allows us to compute the lower bound of the covariance of the estimated meanµˆ to
cov( ˆµ|x)≥ 1
nΣ. (3.106)
Thus, we see that the uncertainty in the estimated mean shrinks with the number of measurements we have.
3.7.2. Cramér-Rao Lower Bound applied to Covariance Intersection
Until now we have investigated the general concept of the CRLB and also its application to measurements from independent Gaussians. Now we apply it in the context of CI. In this case, we are not assuming statistical independence between measurements anymore.
However, the exact correlation between the measurements is unknown. Applying CI
results in a covariance consistent estimate. If we examine the joint probability of the measurements, we obtain similar to (3.99)
lnp(X|µ,Σ) = −1
2(x−Aµ)>(BCI)−1(x−Aµ) +const, (3.107) but hereBCI=diag(ω11Σ, . . . ,ω1
nΣ). Independently of how the weightsωi are chosen we know that by construction Pn
i=1ωi = 1. Using this knowledge and (3.104) we conclude that the FI matrix for the scaled measurements is
ICI(X|µ) = Xn
i=1
Σ−1 ωi
(3.108)
=Σ−1. (3.109)
This allows us to compute the lower bound of the uncertainty for measurements that are fused with CI to
cov( ˆµ|x)≥Σ. (3.110)
It is thus equal to the uncertainty of asinglemeasurement. Therefore, the application of CI does not allow us to increase our certainty by obtaining more measurements. Instead, it aims to provide a covariance consistent estimate and has to implicitly assume that the information content of all measurements equals the information content of a single measurement. This is a conservative assumption that is based on not knowing anything about the correlation between the measurements. In case that we have more knowledge about it we will show in Section 6.4.3 how to exploit this knowledge to attain a lower bound of uncertainty.
4. Architecture of the pose fusion
We present in this chapter the overall architecture of our approach to the pose fusion.
As we are treating pose sources about which we want to make as few assumptions as possible, we divide the main fusion task into two parts. The first part is the core estimator, which makes certain general error assumptions and performs the estimation.
The second part consists of preprocessing techniques that increase the pose estimates’
conformity with the error assumptions of the core estimator. They are applied to selected pose sources. In the following Section 4.1, we detail how these two parts are related to each other.
4.1. Layered fusion architecture
The architecture describes the fundamental partitioning of the pose fusion. This includes the main design decisions, the main components, and the interfaces between these.
Sensor fusion architectures can be categorized into the two major classes: tightly andlooselycoupled approaches (Grewal et al., 2007; Steinhardt and Leinen, 2015; Hol, 2011). On the one hand, tightly coupled approaches directly use all sensor readings to compute the fused result. These approaches usually integrate the data at the position and velocity level. On the other hand, loosely coupled approaches rely on some form of preprocessing of the sensor readings before fusing them. These approaches usually integrate the data at the pseudorange, Doppler, or carrier phase level in the case of GNSS-based fusion (Eling, 2016). Both approaches span a range of fusion schemes in between that differ in the degree that they rely on certain forms of preprocessing.
Figure 4.1 illustrates the key difference between these two architecture patterns for the example of GPS/IMU integration. This specific use case of GNSS/IMU integration is extensively reviewed in the literature. While some comparisons between tightly and
GPS receiver IMU pose fusion
(a) Tight coupling.
GPS receiver IMU
pose solver INS
pose fusion
(b) Loose coupling.
Figure 4.1.: Comparison of tightly and loosely coupled architectures for sensor fusion of data from a GPS receiver and an IMU.
loosely coupled approaches hint at a similar performance (Schwarz et al., 1994), there are others that show advantages for tightly coupled approaches (Scherzinger, 2000).
The architectural choice for one of these two patterns is straightforward for generic pose fusion. This is because its main challenge is that it cannot make specific assump-tions about the sensors, concepts, or implementaassump-tions of underlying pose sources. How-ever, tightly coupled architectures are tailored to specific sensors and their measure-ments. Therefore, we build our generic pose fusion as a loosely coupled approach. This choice promotes the kind of modularity, flexibility, and extensibility that we want to gain from a generic pose fusion.
We approach the design of a loosely coupled system by proposing a layered fusion architecture, as shown in Figure 4.2. Layering is the organization of a system into separate functional components that interact in a hierarchical way. These functional components can be grouped to form a sublayer. Usually, each (sub)layer only has an interface to the layer below and above. Thus, layering is our main tool for complexity reduction and management.