Introduction
In my previous article, I explained how we generate corrected GNSS measurements. Today we take the crucial step: transforming those measurements into estimated position and velocity using the Least Squares algorithm. This isn't just any implementation, it's the mathematical heart of any satellite navigation system.
The Fundamental Problem: Too Many Unknowns
Each pseudorange measurement gives us a nonlinear equation:
ρᵢ = ||p_satᵢ - p_rcv|| + c·δt + εᵢ
Where:
ρᵢ = Measured pseudorange for satellite i
p_satᵢ = Known satellite position
p_rcv = Unknown receiver position (x, y, z)
c = Speed of light
δt = Receiver clock bias
εᵢ = Residual errors
With 4 satellites, we have 4 equations and 4 unknowns. But in practice, we use more satellites, creating an overdetermined system that we solve with** Least Squares**.
Linearization :The Key to Making It Solvable
The problem is that our equations are nonlinear (due to the ||·|| norm). The solution is to linearize around an initial estimate:
Δρ = G·Δx + ε
Where:
Δρ = Vector of residuals (measured - calculated)
G = Geometry matrix (Jacobian)
Δx = Correction to our state estimate
ε = Vector of errors
Detailed Anatomy of the LSQ Algorithm
1. System State
Our state vector has 8 components:
state = [
position_x, // ECEF X coordinate [meters]
position_y, // ECEF Y coordinate [meters]
position_z, // ECEF Z coordinate [meters]
clock_bias, // Receiver clock bias converted to meters [m]
velocity_x, // ECEF X velocity [m/s]
velocity_y, // ECEF Y velocity [m/s]
velocity_z, // ECEF Z velocity [m/s]
clock_drift // Receiver clock drift [m/s]
]
2. The Iterative Process
// Initialize state (often with zeros or approximate position)
current_position_x = 0.0
current_position_y = 0.0
current_position_z = 0.0
current_clock_bias = 0.0
current_velocity_x = 0.0
current_velocity_y = 0.0
current_velocity_z = 0.0
current_clock_drift = 0.0
FOR iteration = 1 TO maximum_iterations DO
// Step 1: Calculate predictions and residuals for all satellites
pseudorange_residuals = []
range_rate_residuals = []
geometry_matrix_pseudorange_rows = []
geometry_matrix_range_rate_rows = []
FOR EACH satellite IN visible_satellites DO
// Calculate satellite-to-receiver geometry
delta_x = satellite_position_x - current_position_x
delta_y = satellite_position_y - current_position_y
delta_z = satellite_position_z - current_position_z
geometric_distance = SQUARE_ROOT(delta_x² + delta_y² + delta_z²)
// Unit vector pointing from receiver to satellite
unit_vector_x = delta_x / geometric_distance
unit_vector_y = delta_y / geometric_distance
unit_vector_z = delta_z / geometric_distance
// Predicted pseudorange = geometric distance + clock bias
predicted_pseudorange = geometric_distance + current_clock_bias
// Residual = measured - predicted
pseudorange_residual = measured_pseudorange - predicted_pseudorange
// Geometry matrix row for pseudorange measurement
pseudorange_geometry_row = [
-unit_vector_x, -unit_vector_y, -unit_vector_z, // Position partials
1.0, // Clock bias partial
0.0, 0.0, 0.0, 0.0 // Velocity and clock drift don't affect pseudorange
]
// For range rate (Doppler) measurements
delta_velocity_x = satellite_velocity_x - current_velocity_x
delta_velocity_y = satellite_velocity_y - current_velocity_y
delta_velocity_z = satellite_velocity_z - current_velocity_z
// Predicted range rate = relative velocity projection + clock drift
predicted_range_rate = (delta_velocity_x * unit_vector_x +
delta_velocity_y * unit_vector_y +
delta_velocity_z * unit_vector_z) + current_clock_drift
range_rate_residual = measured_range_rate - predicted_range_rate
// Geometry matrix row for range rate measurement
range_rate_geometry_row = [
0.0, 0.0, 0.0, 0.0, // Position and clock bias don't affect range rate
-unit_vector_x, -unit_vector_y, -unit_vector_z, // Velocity partials
1.0 // Clock drift partial
]
// Collect all residuals and geometry rows
ADD pseudorange_residual TO pseudorange_residuals
ADD range_rate_residual TO range_rate_residuals
ADD pseudorange_geometry_row TO geometry_matrix_pseudorange_rows
ADD range_rate_geometry_row TO geometry_matrix_range_rate_rows
END FOR
// Step 2: Build complete least squares system
all_residuals = COMBINE(pseudorange_residuals, range_rate_residuals)
full_geometry_matrix = STACK(geometry_matrix_pseudorange_rows, geometry_matrix_range_rate_rows)
// Step 3: Solve for state corrections
information_matrix = TRANSPOSE(full_geometry_matrix) × full_geometry_matrix
covariance_matrix = INVERSE(information_matrix)
correlation_vector = TRANSPOSE(full_geometry_matrix) × all_residuals
state_corrections = covariance_matrix × correlation_vector
// Step 4: Update state estimate
current_position_x = current_position_x + state_corrections[0]
current_position_y = current_position_y + state_corrections[1]
current_position_z = current_position_z + state_corrections[2]
current_clock_bias = current_clock_bias + state_corrections[3]
current_velocity_x = current_velocity_x + state_corrections[4]
current_velocity_y = current_velocity_y + state_corrections[5]
current_velocity_z = current_velocity_z + state_corrections[6]
current_clock_drift = current_clock_drift + state_corrections[7]
// Step 5: Check convergence
correction_magnitude = NORM(state_corrections)
IF correction_magnitude < convergence_tolerance THEN
BREAK // Solution has converged
END IF
END FOR
Mathematical Explanation
Why This Formula Works
We minimize the cost function:
J(Δx) = ‖Δρ - G·Δx‖²
The optimal solution occurs when the gradient is zero:
∇J(Δx) = -2Gᵀ(Δρ - G·Δx) = 0
This leads to the normal equations:
GᵀG·Δx = GᵀΔρ
And finally:
Δx = (GᵀG)⁻¹GᵀΔρ
Geometric Interpretation:
- G projects state corrections from state space to measurement space
- (GᵀG)⁻¹Gᵀ is the pseudo-inverse that finds the minimum-norm solution
- Each iteration brings us closer to the point where residuals are orthogonal to G's column space
Quality Metrics: Dilution of Precision (DOP)
From the covariance matrix we extract essential metrics:
covariance_matrix = (GᵀG)⁻¹
GDOP = SQRT(SUM(diagonal(covariance_matrix)))
PDOP = SQRT(SUM(diagonal(covariance_matrix[0:3,0:3])))
TDOP = SQRT(covariance_matrix[3,3])
DOP Interpretation:
- GDOP < 3: Excellent geometry
- GDOP 3-6: Good geometry
- GDOP > 6: Poor geometry, degraded accuracy
Validation in Local NED Frame
We transform to geodetic coordinates and calculate errors:
estimated_latitude, estimated_longitude, estimated_altitude = CONVERT_ECEF_TO_GEODETIC(
current_position_x, current_position_y, current_position_z
)
estimated_velocity_north, estimated_velocity_east, estimated_velocity_down =
CONVERT_VELOCITY_ECEF_TO_NED(
current_velocity_x, current_velocity_y, current_velocity_z,
estimated_latitude, estimated_longitude
)
// Calculate errors in NED frame
radius_north, radius_east = CALCULATE_EARTH_RADII_OF_CURVATURE(true_latitude)
north_position_error = (estimated_latitude - true_latitude) × (radius_north + true_altitude)
east_position_error = (estimated_longitude - true_longitude) × (radius_east + true_altitude) × COS(true_latitude)
down_position_error = -(estimated_altitude - true_altitude)
horizontal_position_error = SQUARE_ROOT(north_position_error² + east_position_error²)
vertical_position_error = ABSOLUTE_VALUE(down_position_error)
Key Lessons Learned
Initialization: Starting near the true solution significantly speeds up convergence.
Satellite Geometry: The distribution of satellites in the sky (not the number) determines final accuracy.
Numerical Conditioning: When GᵀG is nearly singular (aligned satellites), small measurement errors produce large position errors.
Convergence: Linearization is only valid near the true solution, hence the need for iterations.
Conclusion: Where Theory Meets Practice
Implementing Least Squares from scratch reveals the elegant connection between geometry, linear algebra, and statistical estimation. Every component of the algorithm has deep physical meaning:
- Unit vectors capture directional sensitivity
- G matrix encodes satellite-receiver geometry
- Residuals measure model consistency
- Covariance matrix quantifies inherent uncertainty
This deep understanding is essential before advancing to more sophisticated techniques like the Kalman Filter for sensor fusion.
For those who have worked in navigation: Which theoretical aspects of LSQ did you find most fundamental for robust implementation? How do you handle cases with poor satellite geometry?
Top comments (0)