Nonlinear Least Squares and Differential Corrections

Nonlinear estimation through residual minimization, local linearization, Jacobians, Gauss–Newton iteration, differential correction, convergence, damping, Levenberg–Marquardt, trust-region methods, nonlinear geometry, robust estimation, numerical conditioning, and initial-guess sensitivity.

4.3.5 Nonlinear Least Squares and Differential Corrections

Many spacecraft-estimation problems cannot be written globally in the linear form

\[ \mathbf z=\mathbf H\mathbf x+\mathbf v. \]

Instead, the observations depend nonlinearly on the unknown state or parameters:

\[ \boxed{ \mathbf z = h(\mathbf x,\boldsymbol{\theta}) + \mathbf v } \]

Examples include:

\[ \rho=\|\mathbf r-\mathbf r_s\|, \]

\[ \mathbf u_{\mathrm{LOS}} = \frac{\mathbf r}{\|\mathbf r\|}, \]

In such problems, the residual

\[ \mathbf r(\mathbf x) = \mathbf z-h(\mathbf x) \]

is itself a nonlinear function of the state.

The estimation problem becomes

\[ \boxed{ \hat{\mathbf x} = \arg\min_{\mathbf x} J(\mathbf x) } \]

with

\[ \boxed{ J(\mathbf x) = \mathbf r(\mathbf x)^T \mathbf R^{-1} \mathbf r(\mathbf x). } \]

Unlike linear least squares, the solution normally cannot be obtained from one matrix equation.

Instead, nonlinear least squares proceeds iteratively:

\[ \boxed{ \text{Current Estimate} \rightarrow \text{Linearize} \rightarrow \text{Solve Correction} \rightarrow \text{Update} \rightarrow \text{Relinearize} } \]

The basic idea is that although the complete estimation problem is nonlinear, it may be approximated locally by a linear least-squares problem.

This leads to methods such as:

4.3.5.1 Why Nonlinear Least Squares Is Needed

The linear observation model is

\[ \mathbf z=\mathbf H\mathbf x+\mathbf v. \]

Here, the relationship between observations and unknowns is linear and the design matrix \(\mathbf H\) does not depend on the unknown state.

Many physical measurement models instead have the form

\[ \boxed{ \mathbf z=h(\mathbf x)+\mathbf v. } \]

For example, range is

\[ \rho = \sqrt{ (x-x_s)^2+ (y-y_s)^2+ (z-z_s)^2 }. \]

This cannot be written globally as

\[ \rho = H_xx+H_yy+H_zz. \]

Therefore, the closed-form linear least-squares solution

\[ (\mathbf H^T\mathbf R^{-1}\mathbf H)^{-1} \mathbf H^T\mathbf R^{-1}\mathbf z \]

cannot simply be applied directly.

Instead,

\[ \boxed{ \text{Nonlinear Problem} \rightarrow \text{Local Linear Approximation} \rightarrow \text{Linear LS Correction}. } \]

4.3.5.2 Nonlinear Observation Model

The general nonlinear observation equation is

\[ \boxed{ \mathbf z_k = h_k(\mathbf x,\boldsymbol{\theta}) + \mathbf v_k. } \]

Here,

The predicted measurement for candidate estimate

\[ \mathbf x^{(i)} \]

is

\[ \boxed{ \hat{\mathbf z}^{(i)} = h(\mathbf x^{(i)}). } \]

The corresponding residual is

\[ \boxed{ \mathbf r^{(i)} = \mathbf z-h(\mathbf x^{(i)}). } \]

Because \(h\) is nonlinear, the residual changes nonlinearly as the state changes.

4.3.5.3 Nonlinear Least-Squares Objective

The unweighted nonlinear least-squares problem is

\[ \boxed{ \hat{\mathbf x} = \arg\min_{\mathbf x} \| \mathbf z-h(\mathbf x) \|^2. } \]

Equivalently,

\[ \boxed{ J(\mathbf x) = [ \mathbf z-h(\mathbf x) ]^T [ \mathbf z-h(\mathbf x) ]. } \]

For weighted nonlinear least squares,

\[ \boxed{ J(\mathbf x) = [ \mathbf z-h(\mathbf x) ]^T \mathbf R^{-1} [ \mathbf z-h(\mathbf x) ]. } \]

If multiple observations are available,

\[ \boxed{ J(\mathbf x) = \sum_{k=1}^{N} \mathbf r_k^T \mathbf R_k^{-1} \mathbf r_k. } \]

The goal is to find the state that minimizes this nonlinear cost.

4.3.5.4 Nonlinear Cost Surface

In a linear least-squares problem, the cost function is quadratic.

For nonlinear estimation,

\[ J(\mathbf x) \]

may have a much more complicated shape.

It may contain:

Conceptually,

\[ \boxed{ \text{Nonlinear Measurement Model} \rightarrow \text{Non-Quadratic Cost Surface}. } \]

This is why the initial guess becomes important.

An iterative optimizer usually sees only local information about the cost surface.

Engineering interpretation: The local curvature of the cost surface determines how reliable a linearized correction will be. A narrow curved valley can produce strong parameter correlation and sensitivity to step length even when a unique minimum exists.

4.3.5.5 Initial Guess

Nonlinear least squares requires a starting estimate

\[ \boxed{ \mathbf x^{(0)}. } \]

At this state, the measurement model is evaluated and linearized.

The quality of the starting point can strongly influence:

Thus,

\[ \boxed{ \text{Nonlinear Estimation Requires an Initial Guess}. } \]

Unlike linear least squares, there is generally no guarantee that an arbitrary initial estimate will converge to the desired physical solution.

4.3.5.6 State Correction Form

Instead of solving directly for the complete state, nonlinear least squares typically solves for a correction.

At iteration \(i\),

\[ \boxed{ \mathbf x = \mathbf x^{(i)} + \delta\mathbf x. } \]

Here,

\[ \mathbf x^{(i)} \]

is the current estimate and

\[ \delta\mathbf x \]

is the unknown correction.

The estimator asks:

What small change to the current estimate would best reduce the measurement residuals?

4.3.5.7 Taylor-Series Linearization

Expand the nonlinear observation function around the current estimate:

\[ h(\mathbf x^{(i)}+\delta\mathbf x) \approx h(\mathbf x^{(i)}) + \mathbf H^{(i)} \delta\mathbf x \]

where

\[ \boxed{ \mathbf H^{(i)} = \left. \frac{\partial h}{\partial\mathbf x} \right|_{\mathbf x^{(i)}}. } \]

This is the measurement Jacobian evaluated at the current estimate.

Higher-order terms are neglected.

Thus,

\[ \boxed{ \text{Nonlinear Function} \approx \text{Current Value} + \text{Local Linear Sensitivity}. } \]

Engineering interpretation: The neglected Taylor terms are second order and higher in the correction. This is why first-order linearization is most reliable when the current estimate is already reasonably close to the solution.

4.3.5.8 Linearized Residual

At iteration \(i\),

\[ \mathbf r^{(i)} = \mathbf z-h(\mathbf x^{(i)}). \]

After applying correction \(\delta\mathbf x\),

\[ \mathbf r^{new} = \mathbf z - h(\mathbf x^{(i)}+\delta\mathbf x). \]

Using the first-order approximation,

\[ \mathbf r^{new} \approx \mathbf r^{(i)} - \mathbf H^{(i)} \delta\mathbf x. \]

Therefore,

\[ \boxed{ \mathbf r^{(i)} \approx \mathbf H^{(i)} \delta\mathbf x + \mathbf v. } \]

This is now a linear least-squares problem in the correction \(\delta\mathbf x\).

4.3.5.9 Jacobian Matrix

The Jacobian is

\[ \boxed{ \mathbf H = \frac{\partial h}{\partial\mathbf x}. } \]

For

\[ h: \mathbb R^n\rightarrow\mathbb R^m, \]

the Jacobian has size

\[ \boxed{ \mathbf H\in\mathbb R^{m\times n}. } \]

Its element

\[ H_{ij} = \frac{\partial h_i}{\partial x_j} \]

describes how measurement \(i\) changes when state variable \(j\) changes slightly.

Thus,

\[ \boxed{ \text{Jacobian} = \text{Local Measurement Sensitivity Matrix}. } \]

4.3.5.10 Physical Meaning of the Jacobian

The Jacobian tells the estimator which state corrections can explain the observed residuals.

If

\[ \frac{\partial h_i}{\partial x_j} \approx0, \]

measurement \(i\) provides little local information about \(x_j\).

If two Jacobian columns are nearly parallel, two unknown quantities produce similar measurement effects and may become strongly correlated.

Therefore the Jacobian contains information about:

Engineering interpretation: The Jacobian should therefore be treated as both a solver input and a diagnostic object. Its columns reveal which combinations of state or parameter changes the measurements can distinguish locally.

4.3.5.11 Example: Range Jacobian

Suppose

\[ \rho = \|\mathbf r-\mathbf r_s\|. \]

Let

\[ \boldsymbol{\rho} = \mathbf r-\mathbf r_s. \]

Then

\[ \rho=\|\boldsymbol{\rho}\|. \]

The derivative with respect to spacecraft position is

\[ \boxed{ \frac{\partial\rho}{\partial\mathbf r} = \frac{ \boldsymbol{\rho}^T }{ \|\boldsymbol{\rho}\| }. } \]

This is the line-of-sight unit vector expressed as a row.

Therefore a range measurement is locally sensitive primarily along the line-of-sight direction.

This connects measurement geometry directly to the Jacobian.

4.3.5.12 Gauss–Newton Correction

Using the linearized residual model,

\[ \mathbf r^{(i)} \approx \mathbf H^{(i)} \delta\mathbf x, \]

weighted least squares gives

\[ \boxed{ \mathbf H^{T} \mathbf R^{-1} \mathbf H \, \delta\mathbf x = \mathbf H^{T} \mathbf R^{-1} \mathbf r. } \]

Thus,

\[ \boxed{ \delta\hat{\mathbf x} = ( \mathbf H^T \mathbf R^{-1} \mathbf H )^{-1} \mathbf H^T \mathbf R^{-1} \mathbf r. } \]

The state is then updated:

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \delta\hat{\mathbf x}^{(i)}. } \]

This is the core Gauss–Newton iteration.

4.3.5.13 Why Gauss–Newton Works

The nonlinear cost is

\[ J(\mathbf x) = \mathbf r(\mathbf x)^T \mathbf R^{-1} \mathbf r(\mathbf x). \]

Gauss–Newton approximates this nonlinear cost locally using the first-order residual model.

Near the current estimate,

\[ \mathbf r(\mathbf x+\delta\mathbf x) \approx \mathbf r - \mathbf H\delta\mathbf x. \]

This produces a locally quadratic cost in \(\delta\mathbf x\).

Therefore,

\[ \boxed{ \text{Gauss–Newton} = \text{Repeated Local Weighted Least Squares}. } \]

4.3.5.14 Differential Correction

In orbit determination and trajectory estimation, the iterative Gauss–Newton process is frequently called differential correction.

The idea is

\[ \boxed{ \text{Nominal State} + \text{Estimated Differential Correction} \rightarrow \text{Improved Nominal State}. } \]

At iteration \(i\),

\[ \delta\hat{\mathbf x}^{(i)} \]

is solved and

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \delta\hat{\mathbf x}^{(i)}. } \]

Then the trajectory and measurements are recomputed around the new state.

4.3.5.15 Differential-Correction Loop

A general differential-correction loop is

\[ \boxed{ \begin{array}{c} \text{Initial Guess}\\ \downarrow\\ \text{Propagate / Evaluate Model}\\ \downarrow\\ \text{Predict Measurements}\\ \downarrow\\ \text{Compute Residuals}\\ \downarrow\\ \text{Compute Jacobian}\\ \downarrow\\ \text{Solve Linearized LS Correction}\\ \downarrow\\ \text{Update State}\\ \downarrow\\ \text{Re-evaluate Nonlinear Model}\\ \downarrow\\ \text{Check Convergence}\\ \downarrow\\ \text{Repeat} \end{array} } \]

The repeated relinearization is essential because the Jacobian generally changes with the estimated state.

4.3.5.16 Weighted Gauss–Newton

With measurement covariance \(\mathbf R\), the correction solves

\[ \boxed{ \mathbf N \delta\mathbf x = \mathbf b } \]

where

\[ \boxed{ \mathbf N = \mathbf H^T \mathbf R^{-1} \mathbf H } \]

and

\[ \boxed{ \mathbf b = \mathbf H^T \mathbf R^{-1} \mathbf r. } \]

The matrix \(\mathbf N\) is often called the normal or information matrix.

The correction is

\[ \boxed{ \delta\mathbf x = \mathbf N^{-1}\mathbf b. } \]

In numerical software, one generally solves the linear system without explicitly computing \(\mathbf N^{-1}\).

4.3.5.17 Gradient of the Nonlinear Least-Squares Cost

For

\[ J = \mathbf r^T \mathbf W \mathbf r, \]

with

\[ \mathbf W=\mathbf R^{-1}, \]

and

\[ \mathbf r = \mathbf z-h(\mathbf x), \]

the gradient is

\[ \boxed{ \nabla J = -2 \mathbf H^T \mathbf W \mathbf r. } \]

Therefore the stationary condition is

\[ \boxed{ \mathbf H^T \mathbf W \mathbf r = \mathbf0. } \]

At the optimum, the weighted residual is locally orthogonal to the state-sensitivity directions.

4.3.5.18 Exact Hessian

The exact Hessian of the nonlinear least-squares cost contains two types of terms.

Conceptually,

\[ \boxed{ \nabla^2J = 2\mathbf H^T\mathbf W\mathbf H + \text{Residual-Weighted Second-Derivative Terms}. } \]

The additional terms involve second derivatives of the measurement functions.

Newton's method includes them.

Gauss–Newton neglects them.

4.3.5.19 Gauss–Newton Hessian Approximation

Gauss–Newton approximates the Hessian as

\[ \boxed{ \nabla^2J \approx 2\mathbf H^T\mathbf W\mathbf H. } \]

This approximation is especially reasonable when

Thus Gauss–Newton avoids computing second derivatives.

Engineering interpretation: Gauss–Newton is particularly effective when the residual at the solution is small because the omitted residual-weighted second-derivative terms then contribute less to the exact Hessian.

4.3.5.20 Newton versus Gauss–Newton

Newton's method uses the full Hessian:

\[ \delta\mathbf x = - (\nabla^2J)^{-1} \nabla J. \]

Gauss–Newton uses

\[ \boxed{ \delta\mathbf x = ( \mathbf H^T\mathbf W\mathbf H )^{-1} \mathbf H^T\mathbf W\mathbf r. } \]

Gauss–Newton is often preferred for nonlinear least-squares problems because the residual structure naturally supplies an approximate Hessian.

However, its convergence may be weaker when nonlinearities are severe.

4.3.5.21 Why Relinearization Is Necessary

The Jacobian is evaluated at

\[ \mathbf x^{(i)}. \]

After the state changes,

\[ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \delta\mathbf x, \]

the original Jacobian may no longer accurately describe measurement sensitivity.

Therefore,

\[ \boxed{ \mathbf H^{(i+1)} \neq \mathbf H^{(i)} } \]

in general.

The model must be re-evaluated and relinearized.

4.3.5.22 Local Nature of Linearization

The Taylor approximation

\[ h(\mathbf x+\delta\mathbf x) \approx h(\mathbf x)+\mathbf H\delta\mathbf x \]

is valid only locally.

If

\[ \|\delta\mathbf x\| \]

becomes too large, neglected higher-order terms may become important.

Therefore,

\[ \boxed{ \text{Large Correction} \rightarrow \text{Possible Breakdown of First-Order Linearization}. } \]

This is one reason damping and trust-region methods are useful.

4.3.5.23 Convergence

An iterative estimator converges when repeated corrections lead toward a stable solution.

A typical sequence is

\[ \mathbf x^{(0)}, \quad \mathbf x^{(1)}, \quad \mathbf x^{(2)}, \quad \ldots \]

with

\[ \mathbf x^{(i)} \rightarrow \hat{\mathbf x}. \]

Convergence should not be judged from one quantity alone.

Useful indicators include:

4.3.5.24 Correction-Magnitude Convergence Criterion

One criterion is

\[ \boxed{ \| \delta\mathbf x^{(i)} \| < \epsilon_x. } \]

For states with different units and scales, a normalized criterion may be better:

\[ \boxed{ \| \mathbf S_x^{-1} \delta\mathbf x \| < \epsilon. } \]

This avoids allowing one large-scale state component to dominate the test.

4.3.5.25 Relative Correction Criterion

Another criterion is

\[ \boxed{ \frac{ \| \delta\mathbf x^{(i)} \| }{ \| \mathbf x^{(i)} \| } < \epsilon_r. } \]

This is useful when absolute state scales vary substantially.

However, components near zero require care.

Component-wise normalized convergence can sometimes be more meaningful.

4.3.5.26 Cost-Function Convergence

Convergence may also be assessed by

\[ \boxed{ | J^{(i+1)}-J^{(i)} | < \epsilon_J. } \]

or

\[ \boxed{ \frac{ | J^{(i+1)}-J^{(i)} | }{ J^{(i)} } < \epsilon_{J,r}. } \]

A small cost change indicates that additional iterations are producing little improvement.

But this does not necessarily guarantee that the correct minimum has been found.

4.3.5.27 Gradient-Based Convergence

Because

\[ \nabla J = -2\mathbf H^T\mathbf W\mathbf r, \]

a small gradient can indicate a stationary point:

\[ \boxed{ \| \mathbf H^T \mathbf W \mathbf r \| < \epsilon_g. } \]

This is a stronger mathematical criterion than residual magnitude alone.

4.3.5.28 Maximum-Iteration Limit

Every iterative estimator should include an iteration limit:

\[ \boxed{ i\le i_{\max}. } \]

If the algorithm has not converged by that point, the solution should not simply be declared successful.

Possible causes include:

4.3.5.29 Convergence Does Not Prove Correctness

An optimizer may converge to:

Therefore,

\[ \boxed{ \text{Numerical Convergence} \neq \text{Physical Correctness}. } \]

Residuals, covariance, geometry, parameter correlations, and physical plausibility must also be examined.

Engineering interpretation: A converged numerical solution should be accepted only after the residual pattern, covariance, singular values, parameter correlations, and physical feasibility are all checked.

4.3.5.30 Initial-Guess Sensitivity

The quality of nonlinear estimation can depend strongly on

\[ \mathbf x^{(0)}. \]

If the initial guess is near the correct solution, linearization may be accurate and convergence rapid.

If it is far away, the Jacobian may point in a misleading direction.

Thus,

\[ \boxed{ \text{Initial Guess} \rightarrow \text{Linearization Quality} \rightarrow \text{Convergence Behaviour}. } \]

4.3.5.31 Basin of Attraction

A solution often has a region of initial estimates from which the algorithm converges toward that solution.

This region is called a basin of attraction.

Different local minima may have different basins.

Therefore, nonlinear-estimation robustness can be studied by perturbing the initial guess and checking whether convergence repeatedly reaches the same solution.

4.3.5.32 Multiple Local Minima

A nonlinear cost surface may have several minima:

\[ J(\mathbf x_1^\star), \quad J(\mathbf x_2^\star), \quad \ldots \]

The optimizer may converge to whichever minimum is associated with the initial guess and iteration path.

Therefore,

\[ \boxed{ \text{Nonlinear Optimization} \not\Rightarrow \text{Guaranteed Global Minimum}. } \]

This is especially important when measurement geometry has ambiguities.

4.3.5.33 Poorly Conditioned Jacobian

The Jacobian may be full rank but poorly conditioned.

If columns of \(\mathbf H\) are nearly dependent,

\[ \mathbf H^T\mathbf W\mathbf H \]

can become nearly singular.

Then the computed correction may be very sensitive to small residual changes.

Thus,

\[ \boxed{ \text{Nonlinear Iteration Quality} \text{ depends on both} \text{Model Nonlinearity} + \text{Local Conditioning}. } \]

Engineering interpretation: Poor conditioning can produce very large corrections in weak directions even when the residual vector changes only slightly. Monitoring singular values is often more informative than checking rank alone.

4.3.5.34 Step Overshoot

The full Gauss–Newton step may sometimes be too aggressive:

\[ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \delta\mathbf x. \]

If the linear approximation is poor over the full step length, the new state may actually increase the cost.

Therefore a smaller step may be used.

4.3.5.35 Damped Gauss–Newton

Introduce a damping factor

\[ 0<\alpha\le1. \]

Then

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \alpha \delta\mathbf x^{(i)}. } \]

For

\[ \alpha=1, \]

the full Gauss–Newton step is taken.

For

\[ \alpha<1, \]

the correction is reduced.

This can improve robustness when the local linear approximation is valid only over a limited region.

4.3.5.36 Line Search

Instead of choosing a fixed damping factor, a line search determines an acceptable step length.

Given a search direction

\[ \delta\mathbf x, \]

evaluate

\[ J( \mathbf x+\alpha\delta\mathbf x ) \]

for candidate values of \(\alpha\).

The objective is to choose a step that sufficiently decreases the nonlinear cost.

Conceptually,

\[ \boxed{ \text{Direction} + \text{Step-Length Selection} \rightarrow \text{Safer State Update}. } \]

4.3.5.37 Why Damping Helps

Without damping, a large Gauss–Newton correction may move outside the local region where

\[ h(\mathbf x+\delta\mathbf x) \approx h(\mathbf x)+\mathbf H\delta\mathbf x \]

is accurate.

A reduced step keeps the iteration closer to the region where the Jacobian remains representative.

Therefore,

\[ \boxed{ \text{Damping Trades Convergence Speed for Robustness}. } \]

4.3.5.38 Levenberg–Marquardt Method

Levenberg–Marquardt modifies the Gauss–Newton equations:

\[ \boxed{ ( \mathbf H^T \mathbf W \mathbf H + \lambda\mathbf I ) \delta\mathbf x = \mathbf H^T \mathbf W \mathbf r. } \]

Here,

\[ \lambda\ge0 \]

is a damping parameter.

When

\[ \lambda\rightarrow0, \]

the method approaches Gauss–Newton.

For larger \(\lambda\), the step becomes more conservative and resembles gradient descent.

Engineering interpretation: Levenberg–Marquardt is especially useful when the normal matrix is nearly singular or when the full Gauss–Newton step leaves the region in which the local model is trustworthy.

4.3.5.39 Interpretation of Levenberg–Marquardt

Gauss–Newton is fast near a good solution but may be unstable far away.

Gradient descent is generally more conservative but slower.

Levenberg–Marquardt transitions between these behaviours:

\[ \boxed{ \lambda\text{ small} \rightarrow \text{Gauss–Newton-like} } \]

and

\[ \boxed{ \lambda\text{ large} \rightarrow \text{Gradient-descent-like}. } \]

Therefore,

\[ \boxed{ \text{Levenberg–Marquardt} = \text{Adaptive Compromise Between Speed and Robustness}. } \]

4.3.5.40 Scaling in Levenberg–Marquardt

Instead of

\[ \lambda\mathbf I, \]

a scaled form may be used:

\[ \boxed{ ( \mathbf H^T\mathbf W\mathbf H + \lambda\mathbf D ) \delta\mathbf x = \mathbf H^T\mathbf W\mathbf r. } \]

The matrix \(\mathbf D\) may reflect parameter scaling or diagonal normal-matrix information.

This is important when state components have very different units.

4.3.5.41 Adapting the Damping Parameter

A typical LM algorithm changes \(\lambda\) according to whether the proposed step improves the nonlinear cost.

If the step produces good agreement between predicted and actual cost reduction,

\[ \lambda \]

may be reduced.

If the step performs poorly,

\[ \lambda \]

is increased.

Thus,

\[ \boxed{ \text{Good Local Model} \rightarrow \text{More Gauss–Newton Behaviour} } \]

while

\[ \boxed{ \text{Poor Local Model} \rightarrow \text{More Damping}. } \]

4.3.5.42 Trust-Region Interpretation

Instead of directly damping the normal equations, a trust-region method asks:

How far from the current estimate should we trust the local quadratic approximation?

The correction may be constrained by

\[ \boxed{ \| \delta\mathbf x \| \le \Delta. } \]

Here,

\[ \Delta \]

is the trust-region radius.

4.3.5.43 Trust-Region Logic

If the local model predicts the actual nonlinear cost reduction accurately, the trust region may be enlarged.

If it predicts poorly, the trust region may be reduced.

Conceptually,

\[ \boxed{ \text{Trust-Region Method} \rightarrow \text{Control How Far the Local Linear/Quadratic Model Is Trusted}. } \]

Levenberg–Marquardt has a close relationship to trust-region methods.

4.3.5.44 Actual versus Predicted Cost Reduction

A useful diagnostic compares

\[ \Delta J_{\text{actual}} = J(\mathbf x^{(i)}) - J(\mathbf x^{(i+1)}) \]

with the reduction predicted by the local model.

Define a ratio

\[ \boxed{ \rho = \frac{ \Delta J_{\text{actual}} }{ \Delta J_{\text{predicted}} }. } \]

If

\[ \rho\approx1, \]

the local approximation is good.

If \(\rho\) is small or negative, the proposed local model is unreliable over that step.

Engineering interpretation: The ratio between actual and predicted reduction is a direct test of whether the local quadratic model is representing the true nonlinear cost well over the proposed step.

4.3.5.45 Nonlinear Weighted Least Squares with Prior Information

Suppose a prior state

\[ \bar{\mathbf x} \]

with covariance

\[ \mathbf P_0 \]

is available.

The nonlinear objective is

\[ \boxed{ \begin{aligned} J(\mathbf x) ={}& [ \mathbf z-h(\mathbf x) ]^T \mathbf R^{-1} [ \mathbf z-h(\mathbf x) ] \\ &+ ( \mathbf x-\bar{\mathbf x} )^T \mathbf P_0^{-1} ( \mathbf x-\bar{\mathbf x} ). \end{aligned} } \]

Thus nonlinear estimation can incorporate both observations and a priori information.

4.3.5.46 Linearized Prior-Constrained Correction

Linearization produces

\[ \boxed{ \left( \mathbf H^T \mathbf R^{-1} \mathbf H + \mathbf P_0^{-1} \right) \delta\mathbf x = \mathbf H^T \mathbf R^{-1} \mathbf r + \mathbf P_0^{-1} ( \bar{\mathbf x} - \mathbf x^{(i)} ). } \]

The prior therefore contributes both information and a correction toward the prior state.

4.3.5.47 Nonlinear Parameter Estimation

The unknown vector may contain both states and physical parameters:

\[ \boxed{ \boldsymbol{\xi} = \begin{bmatrix} \mathbf x\\ \boldsymbol{\theta} \end{bmatrix}. } \]

Then

\[ \mathbf z = h(\mathbf x,\boldsymbol{\theta}) + \mathbf v. \]

The Jacobian becomes

\[ \boxed{ \mathbf G = \begin{bmatrix} \frac{\partial h}{\partial\mathbf x} & \frac{\partial h}{\partial\boldsymbol{\theta}} \end{bmatrix}. } \]

The correction is

\[ \delta\boldsymbol{\xi} = \begin{bmatrix} \delta\mathbf x\\ \delta\boldsymbol{\theta} \end{bmatrix}. \]

4.3.5.48 Parameter Identifiability in Nonlinear Estimation

Adding parameters to the unknown vector does not guarantee that they can be estimated.

If

\[ \frac{\partial h}{\partial\theta} \approx0, \]

the parameter is locally weakly observable.

If

\[ \frac{\partial h}{\partial\theta_1} \approx c \frac{\partial h}{\partial\theta_2}, \]

the parameters may be strongly correlated.

Thus,

\[ \boxed{ \text{Parameter Augmentation} \neq \text{Parameter Identifiability}. } \]

4.3.5.49 Dynamic Nonlinear Least Squares

In spacecraft trajectory estimation, measurements may depend on a propagated state:

\[ \mathbf x_k = \phi(t_k,t_0,\mathbf x_0,\boldsymbol{\theta}). \]

Then

\[ \boxed{ \mathbf z_k = h_k[ \phi(t_k,t_0,\mathbf x_0,\boldsymbol{\theta}) ] + \mathbf v_k. } \]

The nonlinear unknown may therefore be the reference state

\[ \mathbf x_0 \]

rather than the instantaneous state at each measurement epoch.

4.3.5.50 Dynamic Jacobian

Using the chain rule,

\[ \boxed{ \frac{\partial h_k} {\partial\mathbf x_0} = \frac{\partial h_k} {\partial\mathbf x_k} \frac{\partial\mathbf x_k} {\partial\mathbf x_0}. } \]

Therefore,

\[ \boxed{ \mathbf G_k = \mathbf H_k \boldsymbol{\Phi}_{k,0}. } \]

This is the same dynamic sensitivity introduced in batch dynamic state estimation.

The nonlinear batch problem is then solved iteratively using differential correction.

Engineering interpretation: For a dynamic trajectory problem, the Jacobian seen by the estimator is therefore not just a sensor derivative; it also includes how a perturbation at the reference epoch propagates through the dynamics to the measurement epoch.

4.3.5.51 Dynamic Differential Correction

At iteration \(i\):

  1. choose reference state

\[ \mathbf x_0^{(i)}, \]

  1. propagate the trajectory,
  2. calculate predicted observations,
  3. form residuals,
  4. propagate the STM,
  5. form dynamic Jacobians,
  6. solve

\[ \delta\mathbf x_0^{(i)}, \]

  1. update

\[ \boxed{ \mathbf x_0^{(i+1)} = \mathbf x_0^{(i)} + \delta\mathbf x_0^{(i)}. } \]

This is the classical batch differential-correction structure used in trajectory and orbit estimation.

4.3.5.52 General Orbit-Determination Example

Suppose the unknown reference state is

\[ \mathbf x_0 = \begin{bmatrix} \mathbf r_0\\ \mathbf v_0 \end{bmatrix}. \]

Tracking observations are available at

\[ t_1,\ldots,t_N. \]

The process is

\[ \boxed{ \mathbf x_0 \rightarrow \text{Orbit Propagation} \rightarrow \mathbf x(t_k) \rightarrow \text{Measurement Prediction} \rightarrow \text{Residuals}. } \]

Because the trajectory and measurement models are nonlinear, the reference orbit is iteratively corrected until the residuals and corrections converge.

4.3.5.53 General RPO Example

Consider a general rendezvous or proximity-navigation problem with relative state

\[ \boxed{ \mathbf x_{\mathrm{rel}} = \begin{bmatrix} \mathbf r_{\mathrm{rel}}\\ \mathbf v_{\mathrm{rel}} \end{bmatrix}. } \]

Suppose a sensor measures range:

\[ \rho = \|\mathbf r_{\mathrm{rel}}\|. \]

Another sensor measures line of sight:

\[ \mathbf u = \frac{ \mathbf r_{\mathrm{rel}} }{ \|\mathbf r_{\mathrm{rel}}\| }. \]

Both measurement equations are nonlinear.

Given a nominal relative state

\[ \mathbf x_{\mathrm{rel}}^{(i)}, \]

the estimator computes:

\[ \hat\rho^{(i)}, \qquad \hat{\mathbf u}^{(i)} \]

and forms residuals.

The measurement models are linearized around the current relative state.

The resulting Jacobian is used to solve a correction

\[ \delta\mathbf x_{\mathrm{rel}}. \]

Then

\[ \boxed{ \mathbf x_{\mathrm{rel}}^{(i+1)} = \mathbf x_{\mathrm{rel}}^{(i)} + \delta\mathbf x_{\mathrm{rel}}. } \]

This process is general and not tied to a particular RPO mission architecture.

4.3.5.54 Why RPO Geometry Can Be Strongly Nonlinear

For line-of-sight measurements,

\[ \mathbf u = \frac{\mathbf r}{\|\mathbf r\|}. \]

As relative range becomes small or geometry changes rapidly, the measurement sensitivity may change substantially with state.

Thus the Jacobian must be evaluated at the correct nominal geometry.

A Jacobian computed far from the true relative state may poorly approximate the actual measurement response.

This makes initial-guess quality and repeated relinearization important.

Engineering interpretation: Changing relative geometry can rapidly rotate the sensitivity directions. In practice, this means a Jacobian that was well conditioned at one configuration may become weak or nearly singular at another.

4.3.5.55 Angle Wrap and Nonlinear Residuals

Angular measurements create special residual issues.

Suppose

\[ \alpha_{\text{meas}} \approx -\pi \]

and

\[ \alpha_{\text{pred}} \approx +\pi. \]

Naively subtracting gives almost

\[ -2\pi \]

even though the physical angular error is small.

Therefore angular residuals should be wrapped consistently:

\[ \boxed{ r_\alpha = \operatorname{wrap} ( \alpha_{\text{meas}} - \alpha_{\text{pred}} ). } \]

Residual definition itself is part of nonlinear-estimation design.

4.3.5.56 Quaternion and Attitude Residuals

Attitude estimation requires similar care.

Directly subtracting quaternion components is often not the most meaningful geometric error.

Instead, one may define a multiplicative attitude error:

\[ \delta\mathbf q = \mathbf q_{\text{true}} \otimes \hat{\mathbf q}^{-1}. \]

A small rotation-vector representation may then be used for local optimization.

Thus,

\[ \boxed{ \text{Nonlinear State Geometry} \rightarrow \text{Choose an Appropriate Local Error Representation}. } \]

4.3.5.57 Constraints in Nonlinear Least Squares

The state may satisfy constraints such as

\[ g(\mathbf x)=0 \]

or

\[ c(\mathbf x)\le0. \]

Examples include:

\[ \|\mathbf q\|=1, \]

Constraints can be enforced through:

4.3.5.58 Parameterization versus Explicit Constraint

Sometimes it is better to choose variables that satisfy the constraint automatically.

For example, attitude may be updated using a three-component local rotation vector rather than estimating four unconstrained quaternion components.

Thus,

\[ \boxed{ \text{Good State Parameterization} \rightarrow \text{Simpler Constraint Handling}. } \]

This can improve both numerical stability and physical interpretability.

4.3.5.59 Numerical Jacobians

If analytical derivatives are difficult, the Jacobian may be approximated numerically.

A central difference is

\[ \boxed{ \frac{\partial h}{\partial x_j} \approx \frac{ h(\mathbf x+\Delta x_j\mathbf e_j) - h(\mathbf x-\Delta x_j\mathbf e_j) }{ 2\Delta x_j }. } \]

This requires repeated model evaluations.

Numerical differentiation is simple but sensitive to the perturbation size.

4.3.5.60 Finite-Difference Step Size

If

\[ \Delta x_j \]

is too large, the finite difference is no longer locally linear.

If it is too small, subtraction and floating-point errors can dominate.

Therefore,

\[ \boxed{ \text{Finite-Difference Step} \text{ must balance truncation error and numerical error}. } \]

Scaling each state variable appropriately is important.

4.3.5.61 Analytical Jacobians

Analytical derivatives provide explicit formulas for

\[ \frac{\partial h}{\partial\mathbf x}. \]

Advantages include:

Disadvantages include:

Analytical Jacobians should be independently verified.

4.3.5.62 Automatic Differentiation

Automatic differentiation evaluates exact derivatives of implemented computations to machine precision, subject to the computational representation.

It can reduce the need for manually derived Jacobians while avoiding many finite-difference errors.

However, it requires compatible software and careful treatment of discontinuities or non-differentiable operations.

4.3.5.63 Jacobian Verification

A good engineering practice is to compare analytical or automatic derivatives with finite-difference approximations.

For column \(j\),

\[ \mathbf H_{:,j}^{analytic} \]

can be compared with

\[ \mathbf H_{:,j}^{FD}. \]

Large differences may indicate:

Engineering interpretation: Derivative verification should be repeated after major model changes. A Jacobian can be mathematically correct for one frame convention or parameterization and become wrong after seemingly small interface changes.

4.3.5.64 Measurement Scaling

Suppose the residual vector contains:

Raw residual magnitudes should not be compared directly.

Weighted least squares uses

\[ \mathbf R^{-1} \]

to normalize residual significance.

Thus,

\[ \boxed{ \text{Different Units} \rightarrow \text{Use Physically Meaningful Covariance Weighting}. } \]

4.3.5.65 State Scaling

The state may contain variables with very different numerical magnitudes.

For example:

\[ \mathbf x = \begin{bmatrix} \text{position}\\ \text{velocity}\\ \text{bias}\\ \text{coefficient} \end{bmatrix}. \]

Define scaled variables

\[ \boxed{ \mathbf x = \mathbf S_x \tilde{\mathbf x}. } \]

This can improve numerical conditioning of

\[ \mathbf H^T \mathbf W \mathbf H. \]

4.3.5.66 Rank Deficiency

At a given iteration,

\[ \operatorname{rank}(\mathbf H)

means the local linearized problem cannot uniquely determine all state directions.

This may result from:

Thus,

\[ \boxed{ \text{Nonlinear Estimator} \text{ still depends on the rank of its local Jacobian}. } \]

4.3.5.67 Changing Rank and Conditioning

In nonlinear estimation, geometry changes with the state.

Therefore the Jacobian may be poorly conditioned at one iteration and much better conditioned at another.

This means estimability can change along the trajectory.

Thus,

\[ \boxed{ \text{Nonlinear Geometry} \rightarrow \text{State-Dependent Conditioning}. } \]

4.3.5.68 Singular Value Analysis

For

\[ \mathbf H = \mathbf U \boldsymbol{\Sigma} \mathbf V^T, \]

small singular values reveal locally weak directions.

If

\[ \sigma_{\min} \approx0, \]

the local problem may be nearly rank deficient.

SVD is therefore useful for diagnosing nonlinear estimator difficulty at each iteration.

4.3.5.69 Covariance Approximation after Convergence

Near the converged solution,

\[ \hat{\mathbf x}, \]

the covariance can often be approximated by

\[ \boxed{ \mathbf P \approx ( \mathbf H^T \mathbf R^{-1} \mathbf H )^{-1}. } \]

This is a local linear covariance approximation.

Its quality depends on how accurately the nonlinear posterior is represented by a locally quadratic cost.

Engineering interpretation: This covariance is local to the converged solution and the adopted linearization. It should not be interpreted as a complete description of global uncertainty when the posterior is strongly nonlinear or multimodal.

4.3.5.70 Limitations of Local Covariance

For strongly nonlinear problems, the true uncertainty distribution may be:

Then one covariance matrix may not fully describe the uncertainty.

Thus,

\[ \boxed{ \text{Local Hessian Covariance} \neq \text{Complete Nonlinear Uncertainty Description}. } \]

Monte Carlo or more general probabilistic methods may be needed.

4.3.5.71 Residual Analysis after Convergence

After convergence,

\[ \boxed{ \mathbf r_{\text{post}} = \mathbf z - h(\hat{\mathbf x}). } \]

The residuals should be examined for:

A converged state with structured residuals may indicate model deficiency.

4.3.5.72 Pre-Fit versus Post-Fit Residuals

At iteration zero,

\[ \boxed{ \mathbf r_{\text{pre}} = \mathbf z-h(\mathbf x^{(0)}). } \]

After convergence,

\[ \boxed{ \mathbf r_{\text{post}} = \mathbf z-h(\hat{\mathbf x}). } \]

Comparing them shows how much of the original observation mismatch is explained by the estimated correction.

However,

\[ \boxed{ \text{Large Residual Reduction} \neq \text{Guaranteed Physical Validity}. } \]

4.3.5.73 Normalized Residuals

Define

\[ \boxed{ \tilde{\mathbf r} = \mathbf R^{-1/2} \mathbf r. } \]

Normalized residuals account for measurement uncertainty.

A raw residual that looks large numerically may be acceptable if the corresponding measurement is noisy.

Conversely, a small raw residual may be statistically significant for a highly precise sensor.

4.3.5.74 Outliers

Squared-error objectives are sensitive to outliers.

An unusually large measurement error can dominate

\[ J. \]

Potential sources include:

Therefore robust outlier handling may be required.

4.3.5.75 Robust Loss Functions

Instead of minimizing purely quadratic residuals,

\[ J = \sum_i r_i^2, \]

robust estimation may use a loss

\[ \boxed{ J = \sum_i \rho(r_i). } \]

Examples include:

These reduce the influence of extreme residuals.

However, robust loss functions change the estimator assumptions and should be justified.

Engineering interpretation: Robust losses intentionally reduce the influence of observations that are inconsistent with the assumed residual model. They should complement, not replace, a physically justified measurement covariance model.

4.3.5.76 Iteratively Reweighted Least Squares

Some robust estimators can be implemented by repeatedly changing measurement weights.

At iteration \(i\),

\[ \boxed{ \mathbf W^{(i)} = \operatorname{diag} ( w_1^{(i)},\ldots,w_m^{(i)} ). } \]

Large residuals may receive reduced weight.

Then another weighted least-squares problem is solved.

This is called Iteratively Reweighted Least Squares, or IRLS.

4.3.5.77 Robust Weighting versus Measurement Covariance Weighting

These should not be confused.

Measurement covariance weighting uses

\[ \boxed{ \mathbf W=\mathbf R^{-1} } \]

based on known or assumed sensor uncertainty.

Robust weighting modifies influence based partly on observed residual behaviour.

Thus,

\[ \boxed{ \text{Covariance Weighting} \neq \text{Outlier-Robust Weighting}. } \]

They solve different problems.

4.3.5.78 Model Error

A nonlinear estimator may converge while compensating for incorrect dynamics or observation models.

If a physical effect is missing, the estimator may distort other state components or parameters to explain the data.

Thus,

\[ \boxed{ \text{Estimator Flexibility Can Hide Model Deficiency}. } \]

Residual structure, parameter plausibility, and independent validation remain essential.

4.3.5.79 Parameter Compensation

Suppose two parameters affect measurements similarly.

The estimator may increase one while decreasing the other and still reduce residuals.

Therefore a low cost does not guarantee accurate individual parameter values.

This is particularly important in nonlinear parameter estimation.

4.3.5.80 Over-Parameterization

Adding more parameters usually gives the optimizer more freedom to fit observations.

But too many parameters may cause:

Thus,

\[ \boxed{ \text{More Estimated Parameters} \neq \text{Better Estimation}. } \]

4.3.5.81 Regularization

A poorly conditioned nonlinear problem can be stabilized through regularization:

\[ \boxed{ J = \mathbf r^T\mathbf W\mathbf r + \lambda \| \mathbf L\mathbf x \|^2. } \]

The corresponding local normal matrix becomes approximately

\[ \boxed{ \mathbf H^T\mathbf W\mathbf H + \lambda \mathbf L^T\mathbf L. } \]

Regularization adds assumptions or prior preference.

It should not be confused with additional measurement information.

4.3.5.82 Levenberg–Marquardt versus Regularization

Both can produce equations containing an additional diagonal term:

\[ \mathbf H^T\mathbf W\mathbf H + \lambda\mathbf I. \]

However, their interpretations differ.

In LM, \(\lambda\) is primarily used to control the optimization step.

In regularized estimation, \(\lambda\) expresses a preference or prior penalty on the solution.

Thus similar algebra does not always imply identical statistical meaning.

Engineering interpretation: The same algebraic term can therefore have two different meanings: algorithmic damping in an optimizer or statistical/physical preference in an estimator. Keeping those interpretations separate avoids misleading uncertainty claims.

4.3.5.83 Hard Constraints and Soft Constraints

A hard constraint must be satisfied:

\[ \boxed{ g(\mathbf x)=0. } \]

A soft constraint is represented with finite uncertainty:

\[ \boxed{ J_c = g(\mathbf x)^T \mathbf R_c^{-1} g(\mathbf x). } \]

A hard constraint reduces the feasible solution space.

A soft constraint allows controlled deviation.

4.3.5.84 Bounds on Parameters

Some parameters must satisfy physical bounds:

\[ \theta_{\min} \le \theta \le \theta_{\max}. \]

Examples include positive scale parameters or bounded calibration terms.

Unconstrained Gauss–Newton may propose physically invalid values.

Bound-constrained optimization or transformed variables can prevent this.

4.3.5.85 Log-Parameter Transformation

For a parameter that must remain positive,

\[ \theta>0, \]

define

\[ \boxed{ \gamma=\ln\theta. } \]

Then

\[ \theta=e^\gamma \]

is automatically positive.

This can eliminate explicit positivity constraints, though it changes the parameter sensitivity.

4.3.5.86 Step Acceptance

A proposed correction should not automatically be accepted merely because it solves the linearized normal equations.

Evaluate the actual nonlinear cost:

\[ J( \mathbf x^{(i)} + \delta\mathbf x ). \]

If the cost increases substantially, the step may need to be rejected, shortened, or damped.

Thus,

\[ \boxed{ \text{Linearized Improvement Prediction} \text{ should be checked against} \text{Actual Nonlinear Improvement}. } \]

4.3.5.87 Monotonic Cost Reduction

Some algorithms are designed so accepted iterations satisfy approximately

\[ \boxed{ J^{(i+1)} < J^{(i)}. } \]

Gauss–Newton without line search does not automatically guarantee this when far from the solution.

Damping or trust-region logic can improve this behaviour.

4.3.5.88 Divergence

An iterative estimator diverges when corrections fail to approach a stable valid solution.

Symptoms include:

Possible causes include:

4.3.5.89 Oscillatory Iteration

Sometimes the estimate alternates between regions:

\[ \mathbf x^{(i)} \leftrightarrow \mathbf x^{(i+1)}. \]

This can occur when corrections overshoot the solution.

Possible remedies include:

4.3.5.90 Stagnation

The estimator may also stagnate:

\[ \|\delta\mathbf x\| \approx0 \]

while the residual cost remains unacceptably large.

Possible reasons include:

Small correction therefore does not automatically mean successful convergence.

4.3.5.91 Initial-Guess Robustness Testing

A practical way to assess nonlinear robustness is to solve the problem from several plausible initial guesses.

For example,

\[ \mathbf x^{(0)}_1, \mathbf x^{(0)}_2, \ldots, \mathbf x^{(0)}_M. \]

Then examine whether the solutions converge to the same physical estimate.

If different starting points lead to substantially different solutions, the problem may contain multiple minima or weak identifiability.

Engineering interpretation: Initial-guess testing is especially valuable before operational use because a single successful run does not reveal the practical convergence basin of the estimator.

4.3.5.92 Monte Carlo Initial-Condition Testing

Initial-guess sensitivity can also be evaluated statistically.

Draw

\[ \mathbf x^{(0,j)} \sim p(\mathbf x_0) \]

and solve repeatedly.

Record:

This measures the practical convergence basin.

4.3.5.93 Covariance versus Convergence Basin

Covariance near the optimum describes local uncertainty.

The convergence basin describes how far the initial estimate may be from the solution while still converging successfully.

These are different concepts:

\[ \boxed{ \text{Local Covariance} \neq \text{Convergence Basin}. } \]

A solution may have a small final covariance but a narrow convergence region.

4.3.5.94 Nonlinear Observability

In nonlinear systems, observability may depend on the state itself.

The local Jacobian may change significantly as the trajectory evolves.

Therefore a state may be weakly observable in one geometry and strongly observable in another.

Nonlinear observability is therefore generally richer than checking a single constant design matrix.

4.3.5.95 Geometry Changes During Iteration

Because the estimated state affects the predicted geometry, an update changes not only the residual but also the measurement sensitivity.

For example,

\[ \mathbf x^{(i)} \rightarrow \mathbf H^{(i)} \]

while

\[ \mathbf x^{(i+1)} \rightarrow \mathbf H^{(i+1)}. \]

Thus,

\[ \boxed{ \text{State Update} \rightarrow \text{Geometry Update} \rightarrow \text{Jacobian Update}. } \]

This is another reason relinearization is mandatory.

4.3.5.96 Measurement Selection

Not every observation necessarily contributes useful independent information.

Measurements may be excluded because of:

However, observations should not be removed simply because they make the residual larger.

Selection criteria should be defined independently and validated.

4.3.5.97 Mixed Measurements

A nonlinear estimator may simultaneously use:

Each has its own nonlinear function

\[ h_j(\mathbf x). \]

The combined residual is

\[ \boxed{ \mathbf r = \begin{bmatrix} \mathbf r_1\\ \mathbf r_2\\ \vdots \end{bmatrix}. } \]

The combined Jacobian is similarly stacked.

4.3.5.98 Asynchronous Measurements in Batch Nonlinear Estimation

Measurements may occur at irregular times.

Each observation uses the state propagated to its own epoch:

\[ \hat{\mathbf z}_k = h_k( \mathbf x(t_k) ). \]

The associated Jacobian must also correspond to the correct measurement epoch.

Thus timestamps are part of the nonlinear measurement model.

4.3.5.99 Numerical Integration Error

For dynamic nonlinear estimation, trajectory propagation is part of the measurement prediction.

If integration error is too large, the predicted measurement contains numerical error.

The estimator may then attempt to compensate by changing the estimated state.

Therefore,

\[ \boxed{ \text{Propagation Numerical Error} \ll \text{Required Estimation Accuracy} } \]

should generally hold.

Engineering interpretation: A practical verification is to tighten the numerical integration tolerances and confirm that the estimated state, cost, and residual history change negligibly.

4.3.5.100 Interpolation Error

Measurements may occur between integration output points.

Then the propagated state may need interpolation.

If interpolation is inaccurate, it can generate artificial residuals.

This can be especially important for high-precision tracking or fast-changing geometry.

4.3.5.101 Numerical Linear Solver

The Gauss–Newton correction is often written analytically as

\[ \delta\mathbf x = ( \mathbf H^T\mathbf W\mathbf H )^{-1} \mathbf H^T\mathbf W\mathbf r. \]

In implementation, avoid explicit inversion.

Possible methods include:

The solver should be selected according to conditioning and problem structure.

4.3.5.102 Why Normal Equations Can Be Risky

Forming

\[ \mathbf H^T\mathbf W\mathbf H \]

can worsen conditioning.

Approximately,

\[ \boxed{ \kappa( \mathbf H^T\mathbf H ) \approx \kappa(\mathbf H)^2. } \]

Thus QR or SVD may be preferable for weakly conditioned problems.

This consideration applies at every nonlinear iteration.

Engineering interpretation: Because the nonlinear problem is re-solved repeatedly, poor numerical conditioning is encountered at every iteration, not just once. A stable linear solver is therefore part of the nonlinear estimator design.

4.3.5.103 Square-Root Nonlinear Least Squares

Instead of forming the normal matrix, the weighted residual problem can be whitened and solved using square-root methods.

If

\[ \mathbf R=\mathbf L\mathbf L^T, \]

define

\[ \tilde{\mathbf r} = \mathbf L^{-1}\mathbf r \]

and

\[ \tilde{\mathbf H} = \mathbf L^{-1}\mathbf H. \]

Then solve

\[ \boxed{ \min_{\delta\mathbf x} \| \tilde{\mathbf r} - \tilde{\mathbf H}\delta\mathbf x \|^2. } \]

This often improves numerical robustness.

4.3.5.104 Sparse Nonlinear Least Squares

Large estimation problems may have sparse Jacobians.

For example, one observation may depend only on a subset of states or parameters.

Sparse numerical methods can exploit this structure to reduce:

This is important in large trajectory, calibration, or multi-arc problems.

4.3.5.105 Nonlinear Least Squares and Factor Graphs

A large estimation problem can also be viewed as a collection of residual factors.

Each factor contributes a residual

\[ \mathbf r_j(\mathbf x) \]

and cost

\[ \mathbf r_j^T \mathbf W_j \mathbf r_j. \]

The total cost is

\[ \boxed{ J = \sum_j \mathbf r_j^T \mathbf W_j \mathbf r_j. } \]

This perspective leads naturally toward factor-graph and graph-optimization formulations.

4.3.5.106 Batch versus Recursive Nonlinear Estimation

Batch nonlinear least squares repeatedly optimizes using the complete observation set.

Recursive nonlinear estimation processes observations sequentially and usually performs local linearization at each update.

Conceptually,

\[ \boxed{ \text{Batch Nonlinear Estimation} \rightarrow \text{Iteratively Optimize an Observation Arc} } \]

while

\[ \boxed{ \text{Recursive Nonlinear Estimation} \rightarrow \text{Propagate and Correct Sequentially}. } \]

4.3.5.107 Iterated Filtering and Gauss–Newton Connection

If a nonlinear recursive estimator repeatedly relinearizes and reprocesses the same measurement, the update increasingly resembles a local Gauss–Newton optimization.

Thus nonlinear least squares and iterated filtering are closely related mathematically.

The difference is largely in:

4.3.5.108 Differential Correction versus EKF Linearization

Both use a Jacobian.

In differential correction,

\[ \mathbf H \]

is used to solve a batch or local least-squares correction.

In an EKF,

\[ \mathbf H \]

is used in a recursive measurement update.

Therefore,

\[ \boxed{ \text{Same Local Linearization Concept} \rightarrow \text{Different Estimation Architecture}. } \]

4.3.5.109 Nonlinear LS versus Maximum Likelihood

Suppose

\[ \mathbf v \sim \mathcal N(\mathbf0,\mathbf R). \]

Then

\[ p(\mathbf z|\mathbf x) \propto \exp \left[ -\frac12 \mathbf r(\mathbf x)^T \mathbf R^{-1} \mathbf r(\mathbf x) \right]. \]

Maximizing likelihood is therefore equivalent to minimizing

\[ \boxed{ J = \mathbf r^T \mathbf R^{-1} \mathbf r. } \]

Thus,

\[ \boxed{ \text{Nonlinear WLS} \Longleftrightarrow \text{Gaussian Maximum Likelihood} } \]

under the corresponding assumptions.

This leads directly toward Section 4.3.6.

4.3.5.110 Nonlinear LS versus MAP

If a Gaussian prior is included,

\[ \mathbf x \sim \mathcal N( \bar{\mathbf x}, \mathbf P_0 ), \]

the cost becomes

\[ \boxed{ J = \mathbf r^T \mathbf R^{-1} \mathbf r + ( \mathbf x-\bar{\mathbf x} )^T \mathbf P_0^{-1} ( \mathbf x-\bar{\mathbf x} ). } \]

This corresponds to Gaussian MAP estimation.

Therefore the nonlinear least-squares framework is directly connected to probabilistic estimation.

4.3.5.111 When Gauss–Newton Works Well

Gauss–Newton generally performs well when:

In this region, convergence can be fast.

4.3.5.112 When Gauss–Newton Struggles

Problems arise when:

In such cases damping or more robust optimization methods may be necessary.

4.3.5.113 Choosing Between Gauss–Newton and Levenberg–Marquardt

Gauss–Newton is attractive when:

Levenberg–Marquardt is attractive when:

Neither is universally superior.

4.3.5.114 Diagnosing a Failed Nonlinear Solve

If nonlinear estimation fails, check systematically:

  1. Are measurements and predictions in the same frame?
  2. Are units consistent?
  3. Are timestamps correct?
  4. Is the residual sign correct?
  5. Is angle wrapping correct?
  6. Is the Jacobian correct?
  7. Is the Jacobian properly scaled?
  8. Is the problem full rank?
  9. Are some parameters strongly correlated?
  10. Is the initial guess reasonable?
  11. Are there outliers?
  12. Is the dynamic model correct?
  13. Is the numerical integration accurate?
  14. Is the step too large?
  15. Is the cost actually decreasing?

Changing the optimizer should not be the first response to a modeling error.

Engineering interpretation: This diagnostic order is important: a more sophisticated optimizer cannot repair an incorrect frame transformation, sign convention, timestamp, Jacobian, or force model.

4.3.5.115 Practical Nonlinear-Estimation Workflow

A disciplined workflow is:

  1. Define unknown state and parameters.
  2. Define coordinate frames and units.
  3. Define nonlinear dynamics.
  4. Define nonlinear observation models.
  5. Define measurement covariance.
  6. Define any prior information.
  7. Obtain a physically reasonable initial estimate.
  8. Propagate the nominal state if dynamic estimation is required.
  9. Generate predicted observations.
  10. Form pre-fit residuals.
  11. Compute measurement Jacobians.
  12. Propagate dynamic sensitivities where required.
  13. Verify Jacobians numerically.
  14. Assemble the weighted design matrix.
  15. Check rank.
  16. Check conditioning.
  17. Apply state and residual scaling.
  18. Solve the Gauss–Newton or damped correction.
  19. Evaluate the nonlinear cost at the candidate state.
  20. Accept, damp, or reject the step.
  21. Update the state.
  22. Recompute trajectory and measurements.
  23. Relinearize.
  24. Repeat until convergence.
  25. Check correction magnitude.
  26. Check cost reduction.
  27. Check gradient magnitude.
  28. Check iteration count.
  29. Compute post-fit residuals.
  30. Examine normalized residuals.
  31. Examine residual trends.
  32. Examine covariance.
  33. Examine parameter correlations.
  34. Test sensitivity to initial guess.
  35. Test alternative initial conditions when appropriate.
  36. Assess physical plausibility.
  37. Validate against independent data or simulation truth where available.

Conceptually,

\[ \boxed{ \begin{array}{c} \text{Initial Guess}\\ \downarrow\\ \text{Evaluate Nonlinear Model}\\ \downarrow\\ \text{Residuals}\\ \downarrow\\ \text{Jacobian / Sensitivities}\\ \downarrow\\ \text{Check Rank and Conditioning}\\ \downarrow\\ \text{Solve Local LS Correction}\\ \downarrow\\ \text{Apply Damping if Needed}\\ \downarrow\\ \text{Evaluate Actual Cost Reduction}\\ \downarrow\\ \text{Update State}\\ \downarrow\\ \text{Relinearize}\\ \downarrow\\ \text{Convergence Check}\\ \downarrow\\ \text{Residual / Covariance / Physical Validation} \end{array} } \]

Engineering interpretation: The development workflow should be iterative. If diagnostics reveal weak observability, bad scaling, model mismatch, or sensitivity to initialization, revise the formulation rather than only increasing the iteration count.

4.3.5.116 Complete Nonlinear Least-Squares Architecture

The complete architecture can be represented as

\[ \boxed{ \begin{array}{c} \text{Current Estimate} \\ \mathbf x^{(i)} \\ \downarrow\\ \text{Nonlinear Measurement Prediction} \\ \hat{\mathbf z}^{(i)} = h( \mathbf x^{(i)} ) \\ \downarrow\\ \text{Residual} \\ \mathbf r^{(i)} = \mathbf z-\hat{\mathbf z}^{(i)} \\ \downarrow\\ \text{Jacobian} \\ \mathbf H^{(i)} = \left. \frac{\partial h} {\partial\mathbf x} \right|_{\mathbf x^{(i)}} \\ \downarrow\\ \text{Linearized Weighted LS Problem} \\ \mathbf H^T \mathbf R^{-1} \mathbf H \, \delta\mathbf x = \mathbf H^T \mathbf R^{-1} \mathbf r \\ \downarrow\\ \text{Correction} \\ \delta\mathbf x^{(i)} \\ \downarrow\\ \text{Step Control / Damping} \\ \downarrow\\ \text{State Update} \\ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \alpha \delta\mathbf x^{(i)} \\ \downarrow\\ \text{Re-evaluate Nonlinear Model} \\ \downarrow\\ \text{Repeat Until Converged} \end{array} } \]

4.3.5.117 Key Equations

Nonlinear measurement model

\[ \boxed{ \mathbf z = h(\mathbf x) + \mathbf v } \]

Residual

\[ \boxed{ \mathbf r(\mathbf x) = \mathbf z-h(\mathbf x) } \]

Nonlinear weighted cost

\[ \boxed{ J(\mathbf x) = \mathbf r(\mathbf x)^T \mathbf R^{-1} \mathbf r(\mathbf x) } \]

State correction form

\[ \boxed{ \mathbf x = \mathbf x^{(i)} + \delta\mathbf x } \]

First-order linearization

\[ \boxed{ h( \mathbf x^{(i)} + \delta\mathbf x ) \approx h( \mathbf x^{(i)} ) + \mathbf H^{(i)} \delta\mathbf x } \]

Jacobian

\[ \boxed{ \mathbf H^{(i)} = \left. \frac{\partial h}{\partial\mathbf x} \right|_{\mathbf x^{(i)}} } \]

Linearized residual

\[ \boxed{ \mathbf r^{(i)} \approx \mathbf H^{(i)} \delta\mathbf x + \mathbf v } \]

Gauss–Newton normal equations

\[ \boxed{ \mathbf H^T \mathbf R^{-1} \mathbf H \delta\mathbf x = \mathbf H^T \mathbf R^{-1} \mathbf r } \]

Gauss–Newton correction

\[ \boxed{ \delta\hat{\mathbf x} = ( \mathbf H^T \mathbf R^{-1} \mathbf H )^{-1} \mathbf H^T \mathbf R^{-1} \mathbf r } \]

State update

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \delta\hat{\mathbf x}^{(i)} } \]

Damped update

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \alpha \delta\hat{\mathbf x}^{(i)} } \]

Levenberg–Marquardt equation

\[ \boxed{ ( \mathbf H^T \mathbf R^{-1} \mathbf H + \lambda\mathbf I ) \delta\mathbf x = \mathbf H^T \mathbf R^{-1} \mathbf r } \]

Trust-region constraint

\[ \boxed{ \| \delta\mathbf x \| \le \Delta } \]

Prior-constrained nonlinear cost

\[ \boxed{ \begin{aligned} J(\mathbf x) ={}& [ \mathbf z-h(\mathbf x) ]^T \mathbf R^{-1} [ \mathbf z-h(\mathbf x) ] \\ &+ ( \mathbf x-\bar{\mathbf x} )^T \mathbf P_0^{-1} ( \mathbf x-\bar{\mathbf x} ) \end{aligned} } \]

Dynamic measurement sensitivity

\[ \boxed{ \frac{\partial h_k} {\partial\mathbf x_0} = \mathbf H_k \boldsymbol{\Phi}_{k,0} } \]

Local covariance approximation

\[ \boxed{ \mathbf P \approx ( \mathbf H^T \mathbf R^{-1} \mathbf H )^{-1} } \]

Finite-difference Jacobian

\[ \boxed{ \frac{\partial h} {\partial x_j} \approx \frac{ h(\mathbf x+\Delta x_j\mathbf e_j) - h(\mathbf x-\Delta x_j\mathbf e_j) }{ 2\Delta x_j } } \]

Correction convergence

\[ \boxed{ \| \delta\mathbf x \| < \epsilon_x } \]

Cost convergence

\[ \boxed{ | J^{(i+1)}-J^{(i)} | < \epsilon_J } \]

4.3.5.118 Key Takeaways

Nonlinear least squares begins with the observation model

\[ \boxed{ \mathbf z = h(\mathbf x)+\mathbf v. } \]

The residual is

\[ \boxed{ \mathbf r = \mathbf z-h(\mathbf x), } \]

and the weighted objective is

\[ \boxed{ J = \mathbf r^T \mathbf R^{-1} \mathbf r. } \]

Because the residual depends nonlinearly on the state, there is generally no single closed-form solution.

Instead,

\[ \boxed{ \text{Nonlinear Estimation} = \text{Sequence of Locally Linear Estimation Problems}. } \]

The nonlinear model is linearized using the Jacobian:

\[ \boxed{ \mathbf H = \frac{\partial h}{\partial\mathbf x}. } \]

The Gauss–Newton correction is obtained from

\[ \boxed{ \mathbf H^T \mathbf R^{-1} \mathbf H \delta\mathbf x = \mathbf H^T \mathbf R^{-1} \mathbf r. } \]

The state is then updated and the entire model is relinearized:

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \delta\mathbf x^{(i)}. } \]

This iterative process is commonly called differential correction in orbit and trajectory estimation.

The initial guess matters because the linear approximation is local:

\[ \boxed{ \text{Poor Initial Guess} \rightarrow \text{Poor Linearization} \rightarrow \text{Possible Divergence}. } \]

Damping can improve robustness:

\[ \boxed{ \mathbf x^{(i+1)} = \mathbf x^{(i)} + \alpha\delta\mathbf x. } \]

Levenberg–Marquardt modifies the local system:

\[ \boxed{ ( \mathbf H^T \mathbf W \mathbf H + \lambda\mathbf I ) \delta\mathbf x = \mathbf H^T \mathbf W \mathbf r. } \]

As

\[ \lambda\rightarrow0, \]

it behaves more like Gauss–Newton.

As \(\lambda\) increases, the correction becomes more conservative.

Trust-region methods express the same basic idea in terms of limiting the region over which the local approximation is trusted.

A converged nonlinear solution should still be assessed using:

Therefore,

\[ \boxed{ \text{Convergence} \neq \text{Proof of Correctness}. } \]

Another central principle is

\[ \boxed{ \text{A Smaller Residual Can Be Produced by the Wrong State if the Model or Parameterization Is Wrong}. } \]

Finally,

\[ \boxed{ \text{Nonlinear Least Squares} \rightarrow \text{Local Linearization} \rightarrow \text{Weighted Correction} \rightarrow \text{Relinearization} } \]

provides the mathematical foundation for a large class of spacecraft trajectory, orbit, navigation, calibration, and parameter-estimation problems.

4.3.5.119 What Comes Next

The nonlinear least-squares formulation has so far been introduced mainly as an optimization problem:

\[ \boxed{ \hat{\mathbf x} = \arg\min_{\mathbf x} \mathbf r(\mathbf x)^T \mathbf R^{-1} \mathbf r(\mathbf x). } \]

But why is this particular cost statistically meaningful?

Suppose measurement uncertainty is Gaussian:

\[ \boxed{ \mathbf v \sim \mathcal N( \mathbf0, \mathbf R ). } \]

Then the probability of observing the measurement data given a candidate state is described by a likelihood function:

\[ p(\mathbf z|\mathbf x). \]

Maximizing that likelihood produces exactly the weighted least-squares solution under Gaussian assumptions.

If prior information is also available,

\[ p(\mathbf x), \]

then estimation becomes

\[ \boxed{ p(\mathbf x|\mathbf z) \propto p(\mathbf z|\mathbf x) p(\mathbf x). } \]

The estimator may then seek either

\[ \boxed{ \hat{\mathbf x}_{ML} } \]

or

\[ \boxed{ \hat{\mathbf x}_{MAP}. } \]

This leads directly to

\[ \boxed{ \textbf{4.3.6 Maximum Likelihood and MAP Estimation} } \]

where the next section should develop:

Next: 4.3.6 Maximum Likelihood and MAP Estimation

Nonlinear least squares has been developed as an optimization problem built from nonlinear residuals, local Jacobians, weighted corrections, damping, convergence control, and repeated relinearization.

The next section gives that objective a probabilistic interpretation. It develops likelihood and log-likelihood, Gaussian measurement models, maximum likelihood estimation, maximum a posteriori estimation, prior and posterior probability, negative log-likelihood, Fisher information, parameter estimation, and the precise relationship between weighted least squares, MLE, and MAP.

4.3.6 Maximum Likelihood and MAP Estimation

Likelihood, log-likelihood, Gaussian observation models, maximum likelihood, maximum a posteriori estimation, priors, posteriors, negative log-likelihood, Fisher information, parameter estimation, and the statistical interpretation of weighted least squares.

Continue to Maximum Likelihood and MAP Estimation →