I have been trying to design nonlinear MPC for attitude control.
I put in the code. runsim is the file you need to run, the rest are just accessories.
This is done with quaternions, where I am using the standard quaternion kinematics along with attitude dynamics. The odd thing is how the cost changes throughout the simulation. I am starting with an initial condition that is nonzero in Euler angle land and nonzero angular velocity about x axis. The goal is just to stabilize to zero Euler angles (unit quaternion) with no angular velocity.
Above you can see how the cost changes along the different time steps.
Below is the corresponding state and control input histories
You should expect that based on these values the cost should be going up the entire simulation. However, there are a couple time steps where it decreases. This is kind of telling me there is something wrong. Anyone have any feedback?