Inconsistent sensor noise handling

[sdunlap-afit]

Hello, I saw #862 was merged and took a look at the sensor implementations. I am noticing some issues/inconsistencies in the latest develop branch. I have only tested a couple of these, the rest were from code review. Some of these may be intentional, but I wanted to point them out.

1. magnetometer.cpp: There is no call to noiseModel.setPropMatrix().
2. coarseSunSensor.cpp:
   • Line 138 still has the 1.5x multiplier for the noise model, but line 159 (fault model) does not. Also, the other sensors no longer have this multiplier.
   • There is no way for the user to set the propagation matrix for the noise model.
3. simpleVoltEstimator.h: Line 56 AMatrix is private, so there's no way for the user to set the propagation matrix for the noise model.
4. starTracker.h: Line 72 AMatrix is also private.
5. imuSensor.cpp: Having to set AMatrix after initializing the simulation is pretty inconvenient. Would it be possible to set it in the constructor instead of in Reset?

Given the above, would it make sense for each sensor to initialize the GaussMarkov object with reasonable defaults in the constructor, and then simply expose (mark public) the GaussMarkov object to the user for further customization? It seems like this would make it easier to keep the various sensors consistent and would make it easier for the user to customize the noise model.

[Natsoulas]

Good catch on coarseSunSensor.cpp! I totally missed that. I'll have to get back to you about your other questions about sensor implementations. Definitely might be good to reconcile them.

[Natsoulas]

Fixed missed sun sensor 1.5x multiplier removal. Will have an update soon on your other items.

[sdunlap-afit]

Excellent, thanks for the quick response!

[Natsoulas]

@sdunlap-afit I will be refactoring sensors' implementation of noise and addressing a good chunk of your items. Thanks again for pointing these inconsistencies out. Do add comments here if anything else relevant comes up.

[sdunlap-afit]

@Natsoulas Thanks for working on this. I was looking at scenarioGaussMarkovRandomWalk.py and noticed that imuSensor1.AMatrixGyro is set before scSim.InitializeSimulation(), so Reset() is reverting it to I. Also, do negative values make sense for a random walk?

[sdunlap-afit]

Also, looking at guass_markov.cpp, "walk bounds" (as it's called in a few places) is really a bound on the total error. If a random walk takes the error to the bound, the error will get clipped (i.e., bound+0 to bound-3sigma). If this is the desired behavior, I recommend avoiding the term "walk bounds" and maybe using "error bounds".

If you want to change the behavior, I think you would just need to move this->currentState += errorVector; below the bounds enforcement. This would ensure that the error about the walk mean is still Gaussian. I would imagine this is the expected behavior for gyro bias.

You could even add another set of bounds, so the user can select the behavior they want.

Current code for reference:

void GaussMarkov::computeNextState()
{
    .....
    //! - Apply noise first
    errorVector = this->noiseMatrix * ranNums;

    //! - Then propagate previous state
    this->currentState = this->propMatrix * this->currentState;

    //! - Add noise to propagated state
    this->currentState += errorVector;

    //! - Apply bounds if needed
    for(size_t i = 0; i < this->numStates; i++) {
        if(this->stateBounds[i] > 0.0) {
            if(fabs(this->currentState[i]) > this->stateBounds[i]) {
                this->currentState[i] = copysign(this->stateBounds[i], this->currentState[i]);
            }
        }
    }
}

[Natsoulas]

@sdunlap-afit Good catch, yes I can change the name to error_bounds, that is more accurate. Settings bounds = -1 is the intentional default for essentially having gaussian noise. Defaulting to disabled random walk was chosen as the default to help enforce users to purposefully set error bounds for their random walk custom to their use case. Lastly, I appreciate your careful review of the gauss markov module and sensor implementations, I'll be sure to ping you when I have changes ready.

[Natsoulas]

Also thanks, I will investigate the Reset() method to ensure expected behavior.

[sdunlap-afit]

@Natsoulas Sorry, for the negative value question, I was referring to the AMatrixGyro value in scenarioGaussMarkovRandomWalk.py

....
    imuSensor1.AMatrixGyro = [
        [-0.1, 0.0, 0.0],
        [0.0, -0.1, 0.0],
        [0.0, 0.0, -0.1]
    ]
....
  scSim.InitializeSimulation()
    
  # Set IMU2's A Matrix to zero AFTER initialization
  imuSensor2.AMatrixGyro = [
      [0.0, 0.0, 0.0],
      [0.0, 0.0, 0.0],
      [0.0, 0.0, 0.0]
  ]

For the Reset() functions, all/most of the sensors set random walk propagation matrix (AMatrix) to I.

void StarTracker::Reset(uint64_t CurrentSimNanos)
{
    // check if input message has not been included
    if (!this->scStateInMsg.isLinked()) {
        bskLogger.bskLog(BSK_ERROR, "starTracker.scStateInMsg was not linked.");
    }

    int numStates = 3;

    this->AMatrix.setIdentity(numStates, numStates);
...

[Natsoulas]

@sdunlap-afit Yes in retrospect the negative here is a random choice. It is not detrimental to the example scenario here, but in best practice for real IMU sensors, negative A matrix values would typically not be set: it leads to abnormally strong mean reversion behavior. For basilisk users, it should be apparent that users should set parameters specific/tailored to desired sensor behavior. In my follow-up pr I'll add a note for users to model sensors with their own desired properties/configs.