M7-T2 — Coupling GNSS and INS: Loose, Tight and Deep

Learning objectives

After this lesson, you should be able to:

• Write the error state of an aided inertial filter, including the clock states.
• Write the measurement equation of a loosely coupled filter (position difference) and a tightly coupled one (pseudorange residual).
• State what each delivers with fewer than four beacons and why.
• Explain why the tight filter must linearize about the corrected position.

The intuition

UAV M3-T11: Multi-sensor fusion architectures compares loose, tight and deep coupling as architectures, and UAV M3-T6: Error-State Kalman Filter derives the filter that runs both. This lesson fills in the equations of the aiding step for Hopper, with a numerical comparison where the architectures differ.

What the filter estimates

The inertial system integrates the sensors and produces a position that drifts. The filter does not estimate the position directly: it estimates the errors of the inertial solution, the position and velocity errors and the accelerometer bias, from a measurement of the difference between the inertial prediction and an outside measurement. The small error state is nearly linear even when the position itself is not, which is why this form is used. In the tightly coupled filter the state also contains the receiver clock bias and its drift, since the pseudoranges depend on them.

Loose coupling: the fix is the measurement

The receiver solves its own fix, as in the last lesson, and the filter takes the difference between the inertial position and the fix as its measurement, with the covariance that the DOP gives. It is simple, the receiver is a black box, and it works with any receiver. It needs at least four beacons: with three the receiver delivers nothing. In the 150 s example the filter gets measurements for the first 40 s, none for the next 60 s, and measurements again afterwards.

Tight coupling: each range is a measurement

The tightly coupled filter takes each pseudorange separately. Its predicted pseudorange is the distance from the inertial position to the beacon, plus the clock bias estimate, and the residual feeds a row of the measurement matrix: minus the unit vector to the beacon on the position error, one on the clock bias. Two beacons give two rows, each of which corrects the position error along its line of sight, and the inertial dynamics fill in the rest. In the example the tight filter ends the outage at 9.4 m against 14.9 m for the loose one, and has an rms of 4.4 m against 7.8 m while only two beacons are visible.

Loose coupling needs a complete fix; tight coupling uses what each beacon gives. They agree with a full constellation, and the tight one degrades more gracefully with fewer beacons. The price is a more complicated filter that needs the raw ranges and the clock states.

Definitions

Error state $\delta\mathbf{x}$

Position error, velocity error, accelerometer bias, and (tight coupling) clock bias and drift: $(\delta p_E, \delta p_N, \delta v_E, \delta v_N, b_E, b_N, \delta c, \delta\dot{c})$.

Loose coupling

The measurement is the position fix computed by the receiver: $\mathbf{z} = \mathbf{p}_{\mathrm{INS}} - \mathbf{p}_{\mathrm{fix}}$, with the covariance of the fix.

Tight coupling

The measurements are the pseudoranges: $z_i = \rho_i - \|\hat{\mathbf{p}} - \mathbf{b}_i\| - \hat{c}$, one per visible beacon.

Deep coupling

The inertial solution also aids the receiver's signal tracking loops; not treated here, but it is the next step along the same axis.

Derivation: the tight measurement row, and why the linearization point matters

The pseudorange residual in terms of the error state.

1. $$\rho_i = \|\mathbf{p} - \mathbf{b}_i\| + \beta + \nu_i$$

   The pseudorange from the true position and the true clock bias.

2. $$\mathbf{p}_{\mathrm{INS}} = \mathbf{p} + \delta\mathbf{p}$$

   The inertial position is the true position plus the error to be estimated.

3. $$\|\mathbf{p}_{\mathrm{INS}} - \mathbf{b}_i\| \approx \|\mathbf{p} - \mathbf{b}_i\| + \mathbf{u}_i^{\mathsf{T}}\delta\mathbf{p}$$

   Linearize the distance about the true position; $\mathbf{u}_i$ is the unit vector from beacon i to the vehicle.

4. $$z_i = \rho_i - \|\mathbf{p}_{\mathrm{INS}} - \mathbf{b}_i\| = \beta - \mathbf{u}_i^{\mathsf{T}}\delta\mathbf{p} + \nu_i$$

   The residual between the measured pseudorange and the one predicted from the inertial position. This gives the row $[-\mathbf{u}_i^{\mathsf{T}},\ldots,1]$.

The linearization point

1. $$\text{error of the linearization} \approx \dfrac{\|\delta\mathbf{p}\|^2}{2\,r_i}$$

   The second-order term dropped above, for a beacon at distance $r_i$. It is small only if the position error is small compared with the range.

2. $$\dfrac{(150\ \mathrm{m})^2}{2\cdot 400\ \mathrm{m}} \approx 28\ \mathrm{m}$$

   With the inertial position 150 m off after the outage and beacons 400 m away, the neglected term is 28 m, the size of the effect to be corrected.

3. $$\mathbf{p}_c = \mathbf{p}_{\mathrm{INS}} - \delta\hat{\mathbf{p}}, \quad z_i = \rho_i - \|\mathbf{p}_c - \mathbf{b}_i\| - \hat\beta$$

   The remedy is to linearize about the position corrected by the current error estimate, and to use the full residual as the innovation (the extended form). The correction then stays small.

The tight measurement row is minus the unit vector to the beacon on the position error and one on the clock. The linearization is only valid about a position that is already close, so the filter must use the corrected position, not the raw inertial one.

A worked example

Hopper moves on a path inside the range for 150 s; three beacons are visible for the first 40 s, two from 40 to 100 s, three again after. The accelerometer bias is (0.03, −0.02) m/s², the clock has a bias of 120 m and a drift of 3 m/s, and the pseudorange noise is 1 m. Results are the rms over 20 noise realizations.

1. Free inertial. The error is 12.9 m rms in the first 40 s, 103.5 m in the next 60 and 293 m after that: the accelerometer bias gives $\tfrac12 bt^2$ of 180 m at 100 s.
2. Three beacons. Loose and tight both give 0.98 m in the first 40 s. From 100 s to 150 s they both settle at about 0.77 m.
3. Two beacons. During the outage the loose filter, with no measurements, runs on its estimate of the bias and has an rms of 7.8 m, and 14.9 m at the last epoch. The tight filter still takes two ranges per second and has 4.4 m rms and 9.4 m at the last epoch.
4. A failure of the naive tight filter. A first version linearized about the inertial position. With the error beyond 100 m at the end of the outage it diverged: 108 m rms after the beacon returned, against 0.77 m once the linearization was done about the corrected position.
5. Reading. The two ranges leave the position unobservable along one direction and the clock correlated with it, so the tight filter does not reach 1 m during the outage, but it halves the loose filter's error and does so without any special logic. A third beacon at any moment restores everything.

import math
import random
import gnckit as g

B = [(-300.0, -200.0), (350.0, -250.0), (400.0, 180.0)]
sig = 1.0                                  # pseudorange noise, m
bt = (0.03, -0.02)                         # true accelerometer bias, m/s2
cb0, cd = 120.0, 3.0                       # clock bias (m) and drift (m/s)
dt = 1.0

def truth(t):
    return (150.0 * math.sin(0.03 * t), 100.0 * math.cos(0.03 * t))

def visible(t):
    return [0, 1, 2] if (t < 40 or t >= 100) else [0, 1]

def mm(A, Bm): return g.mat_mul(A, Bm)

def run(mode, seed):
    rnd = random.Random(seed)
    n = 8 if mode == 'tight' else 6
    # state: dpE dpN dvE dvN bE bN [cb cd]
    F = g.mat_eye(n)
    F[0][2] = dt; F[1][3] = dt; F[2][4] = dt; F[3][5] = dt
    if n == 8: F[6][7] = dt
    Q = [[0.0] * n for _ in range(n)]
    for i, q in ((2, 0.002), (3, 0.002), (4, 1e-8), (5, 1e-8), (6, 0.1), (7, 0.01)):
        if i < n: Q[i][i] = q
    P = [[0.0] * n for _ in range(n)]
    for i, p in ((0, 25.0), (1, 25.0), (2, 0.25), (3, 0.25), (4, 0.0025), (5, 0.0025), (6, 4e4), (7, 100.0)):
        if i < n: P[i][i] = p
    x = [0.0] * n
    xt = [0.0, 0.0, 0.0, 0.0, bt[0], bt[1]]       # true INS error state
    cb = cb0
    out = []
    for k in range(1, 151):
        t = k * dt
        for j in range(2): pass
        xt = [xt[0] + dt * xt[2], xt[1] + dt * xt[3], xt[2] + dt * bt[0], xt[3] + dt * bt[1], bt[0], bt[1]]
        cb += cd * dt
        x = g.mat_vec(F, x)
        P = g.mat_add(mm(mm(F, P), g.mat_T(F)), Q)
        p = truth(t)
        ins = (p[0] + xt[0], p[1] + xt[1])
        vis = visible(t)
        rows, zs, Rd = [], [], []
        innov_direct = None
        if mode == 'tight':
            pc = (ins[0] - x[0], ins[1] - x[1])               # INS position corrected by the current error estimate: the linearization point
            innov_direct = []
            for i in vis:
                b = B[i]
                r = math.dist(p, b)
                u = ((pc[0] - b[0]) / math.dist(pc, b), (pc[1] - b[1]) / math.dist(pc, b))
                pred = math.dist(pc, b) + x[6]
                innov_direct.append((r + cb + rnd.gauss(0, sig)) - pred)
                rows.append([-u[0], -u[1], 0, 0, 0, 0, 1.0, 0.0]); zs.append(0.0); Rd.append(sig * sig)
        elif len(vis) >= 3:
            H = []
            for i in vis:
                b = B[i]; r = math.dist(p, b)
                H.append([(p[0] - b[0]) / r, (p[1] - b[1]) / r, 1.0])
            Q3 = g.mat_inv(g.mat_mul(g.mat_T(H), H))
            nz = [rnd.gauss(0, sig) for _ in vis]
            fix = None
            # solve the fix for this epoch from noisy pseudoranges: linear least squares about the truth is exact enough here
            Hn = g.mat_mul(Q3, g.mat_T(H))
            d = [sum(Hn[a][c] * nz[c] for c in range(len(vis))) for a in range(3)]
            fixpos = (p[0] + d[0], p[1] + d[1])
            rows = [[1.0, 0, 0, 0, 0, 0], [0, 1.0, 0, 0, 0, 0]]
            zs = [ins[0] - fixpos[0], ins[1] - fixpos[1]]
            Rfull = [[sig * sig * Q3[0][0], sig * sig * Q3[0][1]], [sig * sig * Q3[1][0], sig * sig * Q3[1][1]]]
        if rows:
            H = rows
            m_ = len(H)
            if mode == 'tight':
                Rm = [[Rd[i] if i == j else 0.0 for j in range(m_)] for i in range(m_)]
            else:
                Rm = Rfull
            S = g.mat_add(mm(mm(H, P), g.mat_T(H)), Rm)
            K = mm(mm(P, g.mat_T(H)), g.mat_inv(S))
            innov = innov_direct if innov_direct is not None else [zs[i] - sum(H[i][j] * x[j] for j in range(n)) for i in range(m_)]
            x = [x[j] + sum(K[j][i] * innov[i] for i in range(m_)) for j in range(n)]
            IKH = g.mat_sub(g.mat_eye(n), mm(K, H))
            P = g.mat_add(mm(mm(IKH, P), g.mat_T(IKH)), mm(mm(K, Rm), g.mat_T(K)))      # Joseph form keeps P symmetric and positive
            P = [[0.5 * (P[i][j] + P[j][i]) for j in range(n)] for i in range(n)]
        e_ins = math.hypot(xt[0], xt[1])
        e_est = math.hypot(xt[0] - x[0], xt[1] - x[1])
        out.append((t, e_ins, e_est))
    return out

def rms(out, a, b, col):
    seg = [o[col] for o in out if a <= o[0] < b]
    return math.sqrt(sum(v * v for v in seg) / len(seg))

seeds = range(20)
res = {m: [run(m, s) for s in seeds] for m in ('loose', 'tight')}
print('horizontal position error, rms over 20 noise realizations (m):')
print('  phase                    free INS    loosely coupled   tightly coupled')
for name, a, b in (('0-40 s, 3 beacons', 0, 41), ('40-100 s, 2 beacons', 41, 101), ('100-150 s, 3 beacons', 101, 151)):
    ins = math.sqrt(sum(rms(r, a, b, 1) ** 2 for r in res['loose']) / 20)
    lo = math.sqrt(sum(rms(r, a, b, 2) ** 2 for r in res['loose']) / 20)
    ti = math.sqrt(sum(rms(r, a, b, 2) ** 2 for r in res['tight']) / 20)
    print(f'  {name:22s} {ins:9.1f}   {lo:14.2f}   {ti:14.2f}')
print('at the last epoch of the outage (t = 99 s): loose %.2f m, tight %.2f m, free INS %.1f m' % (
    math.sqrt(sum(r[98][2] ** 2 for r in res['loose']) / 20), math.sqrt(sum(r[98][2] ** 2 for r in res['tight']) / 20), math.sqrt(sum(r[98][1] ** 2 for r in res['loose']) / 20)))

horizontal position error, rms over 20 noise realizations (m):
  phase                    free INS    loosely coupled   tightly coupled
  0-40 s, 3 beacons           12.9             0.98             0.98
  40-100 s, 2 beacons        103.5             7.79             4.44
  100-150 s, 3 beacons       292.8             0.76             0.77
at the last epoch of the outage (t = 99 s): loose 14.91 m, tight 9.43 m, free INS 174.9 m

In robotics

Aided navigation is the normal form of navigation for any vehicle with an inertial unit and a position source.

• Which to choose. Loose coupling is easy to build, gives a navigation solution with any off-the-shelf receiver, and is enough when four or more satellites are nearly always visible (open sky, aircraft at altitude). Tight coupling is used where satellites are often blocked: urban canyons, under foliage, near terrain, or any system that must ride through short outages.
• Beyond two beacons. The advantage of tight coupling is largest with one to three beacons, and it ends at zero: with no ranges both are free inertial. Deep coupling extends the aid to tracking loops, so that signals can be followed at lower signal-to-noise ratios and through jamming.
• Observability. The state of the filter is observable only where the measurements and the motion permit it. The clock is separable from height when the geometry is good, and not when it is as flat as Hopper's ground beacons: the measurement design of the last lesson affects the filter too.
• Consistency. The filter's covariance should describe the actual error; a check of the innovations (the integrity lesson of this module) tells whether it does.
• Software. The same structure is in the extended filters of flight software such as PX4's EKF2, which treats GNSS position and velocity as the measurement, and is a loose coupling in the sense used here.

The lab M7-L3: Ride Through the Outage completes the update step of both filters through a 60 s outage with a faulty beacon. The geometry that sets the covariance of the loose measurement is the DOP of M7-T1: The Position Fix: Pseudoranges, Least Squares and Dilution of Precision.

Common misconceptions

Key takeaways

1. The filter estimates the inertial errors: position, velocity, accelerometer bias, and in the tight form clock bias and drift.
2. Loose coupling takes the finished fix as the measurement ($\mathbf{H} = [\mathbf{I},\ \mathbf{0}]$); tight coupling takes each pseudorange ($[-\mathbf{u}_i^{\mathsf{T}},\ \ldots,\ 1]$).
3. With four or more beacons both give about 1 m; with two the tight filter had 4.4 m against 7.8 m rms.
4. At the last epoch of the outage the free inertial error was 175 m, the loose 14.9 m and the tight 9.4 m.
5. The tight filter must linearize about the corrected position: about the raw inertial position it diverged (108 m after the outage).
6. Tight coupling costs more to build and needs the raw ranges and the clock states.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd edition, Artech House, 2013, chapters 14 and 15 on integration architectures. Paid.
• M. S. Grewal, A. P. Andrews and C. G. Bartone, Global Navigation Satellite Systems, Inertial Navigation, and Integration, 4th edition, Wiley, 2020. Paid.
• PX4 documentation, EKF2 tuning , for how a widely used flight stack fuses GNSS with inertial data. Free to read; licence CC BY 4.0.
• MIT 16.322 Stochastic Estimation and Control (OpenCourseWare) , for the Kalman filter used here. Free; licence CC BY-NC-SA 4.0.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T1: The Position Fix: Pseudoranges, Least Squares and Dilution of Precision

  Linearize the pseudorange equations, solve for position and clock bias by iterated least squares, and compute the dilution of precision from the geometry.

• Lab
  M7-L3: Ride Through the Outage

  The fix is lost for 60 s and then returns with one faulty beacon. Complete the measurement update of a loosely and a tightly coupled filter, compare their drift, and reject the fault.

• Lab
  M12-L2: Capstone: Land Hopper

  Write the navigation filter, the guidance law and the controller of Hopper, and pass an acceptance campaign of dispersed flights with wind, sensor errors, mass uncertainty and one beacon fault.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M2-T1: GNSS fundamentals
• UAV M2-T3: GNSS receivers and positioning
• UAV M2-T4: GNSS errors and multipath
• UAV M2-T6: GNSS jamming and spoofing
• UAV M3-T6: Error-State Kalman Filter
• UAV M3-T5: Extended Kalman Filter
• UAV M3-T11: Multi-sensor fusion architectures
• UAV M3-T13: Innovation monitoring
• UAV M3-T16: Sensor fault detection and isolation
• UAV M6-T5: PX4 state estimation and EKF2
• UAV M2-T11: Visual-Inertial Odometry
• SLAM M8-T5: GPS-Denied Localization
• Linear Algebra M5-T6: Weighted Least Squares
• Linear Algebra M6-T5: Singular Value Decomposition
• Probability for Robotics (modules 7, 8 and 12: Kalman, extended and error-state filters)

This course leaves the following to the pages that already teach them:

• Constellations, signals, error sources, multipath, jamming and spoofing
• The Kalman, extended and error-state filter derivations
• Visual, LiDAR and radar odometry, VIO and SLAM as aiding sources
• Innovation tests (NIS, NEES) as such