Implement basic linear kalman filter

[TomD53]

Create a new subsystem "StateEstimator"

The Kalman filter can be a separate class which is a field within the StateEstimator subsystem

The Kalman filter should take inputs of acceleration data (from both IMUs) and output a current estimate of altitude (to be used to deploy parachutes at apogee and a certain height for dual-deploy)

The Kalman filter shall update its estimate every T ms (this is to be decided/designed by you - maybe this is every loop, but it doesn't have to be)

When a new state estimate is calculated, the StateEstimator shall send a message to all subscribed subsystems containing the new state estimate (altitude, velocity, etc)

• Ultimately, we will add EventDetector subsystems to read these messages from the StateEstimator and deploy parachutes/trigger flight events accordingly
• Alternative suggestions are welcome

This task is primarily assigned to @robosam2003 but it's about time we get @kim-bers and @nikiexpo involved.

It would be ideal if a native unit test could be created for the Kalman filter class (not necessarily the state estimator subsystem), so please consider doing this.

Refer to the simpleRocketDynamics.mlx simulink model posted in the "State estimation" thread in December:

Also the following YouTube series: https://www.youtube.com/playlist?list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT

[TomD53]

To begin with, lets start by implementing the most basic state estimator

Inputs:

• Pressure

Outputs:

• Height

The first 100 samples of pressure should be used for calibrating the launch site altitude. The subsystem should take inputs of BarometerDataMsg and output a new SystemMessage called StateEstimateMsg

[TomD53]

Create linear KalmanFilter class that is independent of Subsystem and test it with pio unit test

Use the past launch data from prev karman mini launch

Store the data as a matrix and store the following 3 variables: Time, Y Acceleration, Pressure

Output an estimate of altitude for the entire duration of flight

[nikiexpo]

from discord "oh also just had this thought, as we go up higher in altitude the barometer error will increase so the standard deviation should increase (the more standard deviation the less the filter "trusts" the measurement and after 30km we will want the filter to completely distrust the barometer), right now its constant, which is okay if we are only going 1000m but we will need to sort this for our use case."

[nikiexpo]

Accel STD : 28.16 (MRAS) Altimeter STD: 80.67 (MS5607 with simple pressure to alt conversion )

[nikiexpo]

Adjust for gravity as the output from the accelerometer (which is the control input) is not adjusted for it. So SUBTRACT +1g.

Secondly, the time delta is not 0.03 it's 0.001

[nikiexpo]

Just the Kalman Filter output

The altitude differentiated with respect to time

Which looks similar to the velocity graph, which is expected. As for the noise, not sure but here's the FFT

[TomD53]

Well done Nikilesh you absolute Machine

[nikiexpo]

Leaving some of my comments here so that I don't forget:

1. "WHAT IF THE DATAMSG GET SWITCHED, AS IN THEY COME IN BAROMETERMSG, ACCEL, BARO, BARO THIS WOULD MEAN IT WOULD DO PREDICT TWICE -- DOES THIS MAKE THE SYSTEM MORE ERROR PRONE??"

2. is Nested classes better in any way than just using methods

3. speed analysis on eigen code

[TomD53]

As shown above, when the state estimator starts up the initial altitude estimate is 0. This causes a velocity spike which will impact our launch detection if not properly handled. Therefore, on startup of the state estimator, the barometer will be sampled to set an initial altitude. This will prevent the large velocity spike (see bottom right graph at t=0)