Nonlinear MPC Attitude Control

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?

[Swastik Sarkar]

Hi @Tristan,

There seems to be a typo on line no. 13 and 14

mlobj.Weights.OutputVariables = [1000 1000 1000];
mlobj.Weights.ManipulatedVariables = [1 1 1];

It should be nlobj as that is the name of variable you have used, could this possibly be the cause ?