Magnetometer & Sun Vector Attitude Filter Implementation

[kylekrol]

Magnetometer & Sun Vector Attitude Filter Implementation

Fixes #190.

Summary of changes

• Added GRP attitude representation utility functions to convert to and from utility functions (deferring intensive unit testing to Unit Tests for the GRP Conversion Functions #193).
• Added double and float interfaces for all environment functions (this is helpful for switching between floats and doubles in the filter implementation itself).
• Updated the q_eci_perifocal constant to fix the sun vector model; appropriate unit tests were fixed as well.
• Initial attitude filter implementation when we have both magnetometer and sun sensor readings.
• Simple testcase for the attitude filter with magnetometer + sun sensor readings.

On the surface, the attitude filter seems to work. It's difficult to write good test cases using just PIO because numerical precision, at least so far, has been as issue. For example, the Frobenius norm of the update state covariance matrix is only accurate to two significant digits. Also, we currently only have a single testcase.

There are multiple tickets open to track the work on the attitude filter going forward: #187, #188, #189, #191, #193, and #197. This work includes:

• Developing more testcase and looking at long term filter behavior in MATLAB.
• Improving/verifying numerical stability specifically
• Implementing the magnetometer only Kalman gain update step.
• Adding GRP utility function testcases.

Ptest Effects

NA

Testing

See the added unit test cases and tickets listed above for further testing improvements to come.

Constants

Constant	Location
gnc::constant::ukf_sigma_v	src/gnc_attitude_estimator.cpp
gnc::constant::ukf_sigma_u	sr/gnc_attitude_estimator.cpp
gnc::constant::ukf_sigma_b	src/gnc_attitude_estimator.cpp
gnc::constant::ukf_sigma_s	src/gnc_attitude_estimator.cpp
dt_thresh (local scope)	src/gnc_attitude_estimator.cpp
q_factor (local scope)	src/gnc_attitude_estimator.cpp
var_q (local scope)	src/gnc_attitude_estimator.cpp
var_g (local scope)	src/gnc_attitude_estimator.cpp
init_state (local scope)	src/gnc_attitude_estimator.cpp

Documentation Evidence

See inline documentation and the paper mentioned in the file documentation in include/gnc/attitude_estimator.hpp.

[nhz2]

Can the filter just propagate without sensor readings? Or does the state need to be reset.

[kylekrol]

You can't just propagate without sensor readings, at least that's how it's coded now. In terms of our use case however, there won't be a cc where we don't have at least magnetometer and gyroscope data. If for some reason we don't have that information, we have bigger problems than our attitude filter too lol

[nhz2]

This can be NAN if s_body(0)==0

[kylekrol]

This is a little concerning... However, in cxx a finite float over zero results in plus or minus infinity (IEEE floats at least) and according to the documentation the std::atan function (which is what lin::atan essentially calls) returns pi/2 and -pi/2 for inf and -inf respectively. In theory we should be alright.
This code will get replaced when the new sun vector variance handling scheme is incorporated (issue for that coming soon).

Links:
https://en.cppreference.com/w/cpp/numeric/math/atan
https://stackoverflow.com/questions/4745311/c-division-by-0

[tanishqaggarwal]

Just a couple of comments