3. Fundamentals 27
3.5. Covariance Intersection
X0, linearizing the functional model, and estimating the parameter corrections xˆ by solving
A>P Axˆ =A>P l. (3.69) Note that we can identifyHwithA>P Aandbwith−A>P lby comparing (3.23) with (3.69). Indeed, both approaches solve the same problem, but notation, wording, and formulations are different. For further information on the relationship of graph-based optimization in the robotics community to geodetic mapping approaches, we refer the reader to Agarwal et al. (2014a,b). In the remainder of this thesis, we stick with the graph-based notion on the NLLSQ method as it provides a way of visually reasoning about the relations of observations to parameters.
3.5. Covariance Intersection
CI is an algorithm to fuse two state estimates with cross-correlated but unknown noise.
This is helpful for generic pose fusion where the cross-correlation is typically unknown.
In the following we present in Section 3.5.1 the optimal and naive fusion, followed in Section 3.5.2 by the description of the general CI approach. In Section 3.5.3 and Section 3.5.4 we detail how to derive the closed-form solutions for it. This derivation is extended in Section 6.3 where we use CI to deal with cross-correlated errors.
3.5.1. Optimal and naive fusion of states with cross-correlated noise
In state estimation and fusion problems it is a common task to fuse two state estimates into a single state. This is conventionally achieved by a linear combination of the two input state estimates. If the correlation between the two input state estimates is known, we can derive the optimal fusion result. For this we follow the derivation of Li et al.
(2015); Chen et al. (2002).
Two normally distributed statesx1 andx2 with covariance matricesΣ1 andΣ2 can generally be combined by
x=A1x1 +A2x2 (3.70)
whereA1 andA2 denote quadratic matrices. To ensure that the mean error is equal to zero, A1 +A2 = I must hold. The variance law of error propagation implies for the resulting covariance matrix
Σ =h
A1 A2
i"
Σ1 Σ12
Σ12> Σ2
# "
A>1 A>2
#
(3.71)
=A1Σ1A>1 +A1Σ12A>2 +A2Σ12>A>1 +A2Σ2A>2. (3.72) To obtain a fusion with a small covariance matrix, A1 andA2 are set such that Σ is optimal in some sense, e.g., that Σ has a minimal trace or determinant. The optimal solution with respect to trace minimization yields
Σ =
h
I Ii"
Σ1 Σ12
Σ12> Σ2
#−1"
I I
#
−1
. (3.73)
This is the covariance of the optimal fusion. However, it requires knowledge about the cross-correlation matrix Σ12. If it is unknown, the naive approach is to assume Σ12=0. In that case (3.73) can be simplified to
Σ = Σ1−1+Σ2−1,−1
(3.74) which corresponds to the Kalman Gain as defined in a classical Kalman filter. This, however, leads to overconfident estimates ifΣ12is positive definite. We use this knowl-edge about the optimal and naive fusion to evaluate our CI framework.
The error of the naive fusion can be estimated from (3.72) withA1Σ12A>2 +A2Σ12>A>1. If the noise of the estimates correlate with a correlation coefficientρ, i.e.,Σ12=ρI, the error is linear inρ. If we can find an upper bound for the cross-correlation, we can de-cide whether the error is small and tolerable or if we need to take the cross-correlation into account. In general, however, we need a method to produce conservative uncer-tainty estimates without knowledge of their cross-correlation. For this we detail the CI framework in Section 3.5.2.
3.5. Covariance Intersection 65
3.5.2. Fusion with unknown cross-correlation
CI is the statistically optimal algorithm to fuse two estimates x1,x2 with associated covariance matrices Σ1,Σ2 if the cross-correlations Σ12 between their errors are un-known (Uhlmann, 1995). The resulting covariance matrix Σω and the fused statexω are computed according to
Σω = ωΣ1−1+ (1−ω)Σ−12 −1
, (3.75)
xω =Σω ωΣ1−1x1+ (1−ω)Σ2−1x2
(3.76) withω ∈[0,1]. The parameterωis chosen in such a way that the covariance matrixΣω is minimized regarding a given objective functionJ such that
ω∗ = arg min
ω∈[0,1]
J(Σω) (3.77)
in order to minimize the upper bound of the corresponding mean square error matrix.
Common choices for J are the trace and determinant of Σω. We evaluate these two choices on simulated data in Section 7.4.3 and show that both choices forJare suitable for the problem of pose fusion. For low-dimensional fusion problems, Reinhardt et al.
(2012) have derived closed-form solutions for both choices ofJ. We briefly highlight the joint diagonalization approach in Section 3.5.3 and the derivation of the closed-form solutions in Section 3.5.4.
3.5.3. Joint diagonalization
Joint diagonalization for matrices is a useful tool to derive closed-form solutions for the CI optimization. The covariance matrices are transformed in such a way that one of them becomes the identity while the other one becomes a diagonal matrix.
LetE1,E2 be the diagonal matrix containing the eigenvalues ofΣ1,Σ2. The corre-sponding eigenvector matrices areV1,V2. Therefore, fori= 1,2we have
Σi =ViEiVi> =Vi
pEi
pEiVi>. (3.78)
ThenΣ1 can be transformed to the identity matrixI usingT1 = (V1√
E1)−1:
Σ10 =T1Σ1T1> =I, Σ20 =T1Σ2T1>. (3.79) The eigenvalue decompositionΣ20 = V20E20(V20)> can now be used to diagonalize the second covariance matrix. WithT = (V20)>T1 we obtain
Σ100=T Σ1T> = (V20)>Σ10V20 = (V20)>V20 =I (3.80) Σ200=T Σ2T> = (V20)>T1Σ2(T1)>V20 = (V20)>Σ20V20 =E20. (3.81) We will use this result in Section 3.5.4 to derive the closed-form solutions for low-dimensional CI problems.
3.5.4. Closed-form solutions
We follow the work of Reinhardt et al. (2012) to derive the closed-form solutions. Ap-plying the joint diagonalization onΣω yields
T ΣωT>= (ωI + (1−ω)(E20)−1)−1. (3.82) ForJ(·) = det(·)the minimization problem (3.77) is equivalent to
ω∗ = arg min
ω∈[0,1]
det(T ΣωT>), (3.83) which follows directly from the determinant rules and that T is regular and does not depend onω.
Letdi be the diagonal elements ofE20, i.e., the eigenvalues ofΣ20 andd¯i = d1
i. Then the minimization problem (3.83) can be expressed as
ω∗ = arg max
ω∈[0,1]
Y
i∈I
(ω+ (1−ω) ¯di) (3.84)
whereI ={1, ..., n}. Usingd˜i = 1−d¯id¯
i the explicit solution forn = 2is given as ω =−1
2( ˜d1+ ˜d2). (3.85)
3.5. Covariance Intersection 67 The two candidates forn = 3are
ω1/2 =−1
3( ˜d1+ ˜d2 + ˜d3)±
qd˜21+ ˜d22+ ˜d23−d˜1d˜2−d˜1d˜3−d˜2d˜3. (3.86)
Note that the minimization problem (3.77) is convex on the interval[0,1]and therefore there will only be one valid solution in this interval.
Respectively, we get
ω∗ = arg min
ω
X
i∈I
ai
ω+ (1−ω) ¯di
(3.87)
in case of trace minimization whereai >0denotes thei-th diagonal element of(T>)−1T−1. The roots of the derivative of the polynomial are given as
ω1 =−p+p
p2−q, andω2 =−p−p
p2−q (3.88)
with
p= a1d˜2(1 + ˜d1) +a2d˜1(1 + ˜d2)
a1(1 + ˜d1) +a2(1 + ˜d2) , (3.89) q = a1( ˜d2)2(1 + ˜d1) +a2( ˜d1)2(1 + ˜d2)
a1(1 + ˜d1) +a2(1 + ˜d2) . (3.90) With these two derivations we are capable of fusing two state estimates with closed-form solutions of two variants of CI.
3.5.5. Weighted Geometric Mean
So far we have considered the application of CI to two measurements. Most of the time this is sufficient for our use case. However, in Section 3.7.2 we will need to apply this concept to multiple measurements. To this end, we present a generalization of CI to multiple non-Gaussian probability density functions (PDFs) that is called WGM6. From this generalization we will recover the CI fusion rule for multiple Gaussian
measure-6Sometimes, it is also referred to as Normalized Weighted Geometric Mean (Bailey et al., 2012), Weighted Geometric Density (Julier, 2012), or Generalized Uhlmann-Julier-Covariance Intersec-tion(Mahler, 2000).
ments as a special case.
The WGM fusion rule forN PDFspi(X)is
p(X) = 1 η
YN i=1
pi(X)ωi (3.91)
withη begin a normalization constant, ωi ≥ 0, and P
iωi = 1. Note that we did not need to specify a type of distribution. For the particular case of Gaussian distributions the effect of raising the PDF to a power and normalizing it is the same as multiplying its covariance matrix with the inverse power. To see this, suppose thatXi ∼ N(µi,Σi) with PDFpi(X). Then, η1ipi(X)ωi is equivalent toXi ∼ N(µi,Σωi
i). We use this result in combination with (3.91) to recover the CI fusion rule forN Gaussian measurements:
Σω = XN
i=1
ωΣi−1
!−1
, (3.92)
xω =Σω XN
i=1
ωΣi−1xi
!−1
. (3.93)
Again,ωi ≥0andP
iωi = 1. Therefore, the detour of generalizing CI to WGM allows us to derive the CI fusion rule for multiple measurements.