Code for Dynamic Flight Rocket Control

My team and I are building a model rocket with dynamic fin control throughout flight. We need to write a code that will relay information on the rockets position in order for an ESP32 to adjust fin angle to correct for change in flight. We are using 3 Corona DS843MG servos, an ESP32, a BNO055 IMU, BPM388 altimeter, and ZOE-M8 GNSS. We are trying to use ODE45 for the code.

We are also trying to incorporate a Kalman filter into the code for better control during flight. I was wondering if anybody could give me pointers in writing the Matlab code or possibly a template to work with. If so that would be amazing as I am trying to learn Matlab while writing this code and don't have much time.

[Aasmund]

Did you publish anything on the topic, I'm Writing my master thesis on a similar topic now