Weights in least squares estimation

[tomastel]

Hello,

I am trying to estimate the rotation between two vectors in R^3 with a least squares estimator. As shown in one of my residuals below, I am the function to_tangent() to calculate the residual. How can I incorporate uncertainties/weights to make a weighted least squares estimator? What do the elements in the to_tangent-element represent?

Any help is much appreciated.

def sun_angle_residual(R_n_b: sm.Rot3, sun_b: sm.V3, sun_n_almanac: sm.V3, epsilon: sm.Scalar):
      sun_n_pred = R_n_b*sun_b
      sun_n_pred_unit = geo.Unit3.from_vector(sun_n_pred)
      sun_n_almanac_unit  = geo.Unit3.from_vector(sun_n_almanac)
      diff = sun_n_almanac_unit.between(sun_n_pred_unit)
        
      return diff.to_tangent()

[aaron-skydio]

What do the elements in the to_tangent-element represent?

The tangent space for Rot3 is equivalent to the compressed angle-axis vector, i.e. the direction of the tangent vector is the axis of rotation and the norm is the angle of rotation - the elements are the x, y, and z components of that vector

How can I incorporate uncertainties/weights to make a weighted least squares estimator?

For simple weights, you can multiply the result by the weight (or the sqrt of the weight, if you want to pass in the weight on the cost rather than the residual), e.g.:

def sun_angle_residual(R_n_b: sm.Rot3, sun_b: sm.V3, sun_n_almanac: sm.V3, weight: sm.Scalar, epsilon: sm.Scalar):
      sun_n_pred = R_n_b*sun_b
      sun_n_pred_unit = geo.Unit3.from_vector(sun_n_pred)
      sun_n_almanac_unit  = geo.Unit3.from_vector(sun_n_almanac)
      diff = sun_n_almanac_unit.between(sun_n_pred_unit)
        
      return sm.sqrt(weight) * diff.to_tangent()

This is equivalent to symforce.opt.noise_models.IsotropicNoiseModel, there are other noise models in that module that you can also use for more complicated things

[tomastel]

Thank you for your answer. What is the tangent space for Unit3? I would like to, in some way, weigh the elements in the residual according to the uncertainties of each element in my input vector.

[aaron-skydio]

Tangent space for Unit3 is similar - the norm is the angle the vector is moved. The direction corresponding to the two elements is sort of an implementation detail, but each element corresponds to movement along one direction in the tangent plane to the sphere (the actual Unit3 internals can have different values to represent the same unit vector, and the directions of the two elements of the tangent space depend on those values)

[aaron-skydio]

Ah, I guess that doesn't really answer your question. If you have a covariance on sun_n_almanac, you could project that to a covariance on the result with something like this I think?

def sun_angle_residual(R_n_b: sm.Rot3, sun_b: sm.V3, sun_n_almanac: sm.V3, sun_n_almanac_cov_diag: sm.V3, epsilon: sm.Scalar):
      sun_n_pred = R_n_b*sun_b
      sun_n_pred_unit = geo.Unit3.from_vector(sun_n_pred)
      sun_n_almanac_unit  = geo.Unit3.from_vector(sun_n_almanac)
      diff = geo.V2(sun_n_almanac_unit.local_coordinates(sun_n_pred_unit))
      diff_D_sun_n_almanac = diff.jacobian(sun_n_almanac)
      diff_cov = diff_D_sun_n_almanac * geo.M33.diag(sun_n_almanac_cov_diag) * diff_D_sun_n_alm
anac.T
      diff_sqrt_info = geo.M22(diff_cov.inv().mat.cholesky())
      return diff_sqrt_info * diff

For some reason I can't really come up with a way to do this more nicely, but the 2x2 inverse there should be fine (also cholesky isn't something we expose on geo.Matrix, which is why I'm using the internal mat there)

[tomastel]

Thank you for the useful response!