Known landmark localization in ℝ³

This example demonstrates 3D localization of a single point state $x$ from four known landmarks using KnownLandmarkSquaredRangeFactor3.

A single unknown point $x \in \mathbb{R}^3$ is observed through range measurements to several fixed, known landmarks $L_i$ . A conventional range residual has the form

e_i(x) = \|x - L_i\| - d_i,

(1)

where $d_i$ is the measured range to landmark $L_i$ . Because the Euclidean norm contains a square root, this residual is not polynomial in the state.

KnownLandmarkSquaredRangeFactor2 and KnownLandmarkSquaredRangeFactor3 instead use the squared-range residual

e_i(x) = \|x - L_i\|^2 - d_i^2,

(2)

which is quadratic in $x$ . This polynomial form is suitable for lifted QCQP formulations and subsequent semidefinite relaxations. If the semidefinite relaxation is tight, meaning that the lifted solution has the required rank, the relaxed problem recovers a globally optimal solution to the original QCQP.

In this notebook, the factor is used with GTSAM’s standard nonlinear optimizer; QCQP/certifiable conversion is not included.

Several geometrically distinct landmarks are required to uniquely determine the point. This example uses four known landmarks and noisy range measurements to estimate the 3D position.

See the KnownLandmarkSquaredRangeFactor API notebook for more details on the factor.

import numpy as np
import gtsam
import plotly.graph_objects as go

Create a 3D synthetic localization problem¶

We choose one true 3D point and four fixed landmarks. The landmarks are spread through space so that the resulting range measurements provide independent geometric constraints.

Gaussian noise is added to each raw range measurement for realism.

rng = np.random.default_rng(42)

x_true = np.array([1.0, 2.0, 3.0])

landmarks = np.array([
    [0.0, 0.0, 0.0],
    [5.0, 0.0, 0.0],
    [0.0, 6.0, 0.0],
    [1.0, 0.0, 8.0],
])

range_sigma = 0.10  # raw range standard deviation [m]

true_ranges = [np.linalg.norm(x_true - wL) for wL in landmarks]

measurement_noise = rng.normal(
    loc=0.0, scale=range_sigma, size=len(landmarks)
)

measured_ranges = true_ranges + measurement_noise

print("True point:", x_true)
print(f"Raw range standard deviation: {range_sigma:.3f} m\n")

for i, (wL, r_true, r_measured) in enumerate(
    zip(landmarks, true_ranges, measured_ranges)):
    print(
        f"L{i}: {wL} "
        f"True range = {r_true:.3f} m, ",
        f"Measured range = {r_measured:.3f} m"
    )

True point: [1. 2. 3.]
Raw range standard deviation: 0.100 m

L0: [0. 0. 0.] True range = 3.742 m,  Measured range = 3.772 m
L1: [5. 0. 0.] True range = 5.385 m,  Measured range = 5.281 m
L2: [0. 6. 0.] True range = 5.099 m,  Measured range = 5.174 m
L3: [1. 0. 8.] True range = 5.385 m,  Measured range = 5.479 m

Propagate range uncertainty into squared-range units¶

The sensor uncertainty above is expressed in range units, while KnownLandmarkSquaredRangeFactor3 uses the squared-range residual

e_i(x) = \|x - L_i\|^2 - d_i^2.

(3)

For $y = d^2$ , first-order uncertainty propagation gives

\sigma_y \approx \left|\frac{\partial d^2}{\partial d}\right|\sigma_d = 2d\,\sigma_d.

(4)

Therefore, the standard deviation used by each factor is approximately

\sigma_{d^2} \approx 2d\,\sigma_d.

(5)

Even when each range measurement has the same $\sigma_d$ , their respective squared-range uncertainties differ between landmarks.

Build the factor graph¶

Each range measurement contributes one factor to the same unknown point state.

key = 0
graph = gtsam.NonlinearFactorGraph()

for wL, measured_range in zip(landmarks, measured_ranges):
    sigma_squared_range = 2.0 * measured_range * range_sigma
    model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_squared_range)
    graph.add(
        gtsam.KnownLandmarkSquaredRangeFactor3(key, wL, measured_range, model)
    )

Choose an initial estimate¶

The nonlinear optimizer needs an initial value for the point state. The initial position is deliberately offset from the true position.

x_initial = np.array([1.2, 1.7, 3.2])

initial = gtsam.Values()
initial.insert(key, x_initial)

Optimize¶

We solve the nonlinear least-squares problem with Levenberg-Marquardt.

optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial)
result = optimizer.optimize()

x_estimated = result.atVector(key)

print("True point: ", x_true)
print("Initial point: ", x_initial)
print("Estimated point: ", x_estimated)
print("Final graph error: ", graph.error(result))
print("Localization error: ", np.linalg.norm(x_estimated - x_true))

True point:  [1. 2. 3.]
Initial point:  [1.2 1.7 3.2]
Estimated point:  [1.14882058 1.99174407 2.96859758]
Final graph error:  0.41722601045710256
Localization error:  0.15232149470799192

Visualize the result¶

The interactive 3D plot below shows the known landmarks, true position, initial estimate, and optimized estimate. Rotate or zoom the plot to inspect the geometry from different viewpoints.

fig = go.Figure()

# Known landmarks
fig.add_trace(
    go.Scatter3d(
        x=landmarks[:, 0],
        y=landmarks[:, 1],
        z=landmarks[:, 2],
        mode="markers",
        marker=dict(size=6),
        name="Known Landmarks",
        hovertemplate=(
            "Landmark<br>"
            "x=%{x:.2f}<br>"
            "y=%{y:.2f}<br>"
            "z=%{z:.2f}<extra></extra>"
        ),
    )
)

# True position
fig.add_trace(
    go.Scatter3d(
        x=[x_true[0]],
        y=[x_true[1]],
        z=[x_true[2]],
        mode="markers",
        marker=dict(size=7),
        name="True Position",
    )
)

# Initial estimate
fig.add_trace(
    go.Scatter3d(
        x=[x_initial[0]],
        y=[x_initial[1]],
        z=[x_initial[2]],
        mode="markers",
        marker=dict(size=7),
        name="Initial Estimate",
    )
)

# Optimized estimate
fig.add_trace(
    go.Scatter3d(
        x=[x_estimated[0]],
        y=[x_estimated[1]],
        z=[x_estimated[2]],
        mode="markers",
        marker=dict(size=7),
        name="Estimated Position",
    )
)

# Localization error line
fig.add_trace(
    go.Scatter3d(
        x=[x_true[0], x_estimated[0]],
        y=[x_true[1], x_estimated[1]],
        z=[x_true[2], x_estimated[2]],
        mode="lines",
        name="Estimation Error",
    )
)

fig.update_layout(
    title="3D Squared-Range Localization",
    width=800,
    height=650,
    scene=dict(
        xaxis_title="x",
        yaxis_title="y",
        zaxis_title="z",
        aspectmode="data",
    ),
    legend=dict(
        x=0.02,
        y=0.98,
    ),
    margin=dict(l=0, r=0, b=0, t=50),
)

fig.show()

Takeaways¶

KnownLandmarkSquaredRangeFactor3 estimates a 3D Euclidean point, not a pose.
Each factor relates the point to one fixed known landmark.
Pass the non-negative range measurement $d$ to the constructor; the factor stores $d^2$ internally.
The residual is scalar, so each factor uses a one-dimensional noise model.
Raw range uncertainty should be propagated into squared-range units before use in the factor noise model.
Known landmarks are fixed factor data and are not inserted into Values.