How can I keep the quaternion state normalized at every step in a MATLAB S-function, not just at the output?

Hello,

I am simulating the dynamics of a quadrotor using a MATLAB S-function. The attitude is represented with a quaternion. I normalize it in mdlOutputs. However, I also need the quaternion to be normalized after each integration/update step, because the internal state vector x (which contains the quaternion) is what gets fed back into the dynamics.

I'm searching for something like "mdlUpdate", but it's used for discrete states.

Can you please advice me on how should I proceed?

Thanks in advance!

[Sam Chak]

Hi @NY AINA HENINTSOA

You may need to use the normalized quaternion differential equations. However, I would suggest testing the basic integration code in MATLAB using the ode45 solver before implementing it in the Simulink S-function.

Could you provide the MATLAB code for basic attitude dynamics using the normalized quaternion differential equations? You can click the "play" icon to run the code in this forum.

function dx = normAttitude(t, x)
    dx  = [];
end

[t, x]  = ode45(@normAttitude, tspan, x0);

Answer 1

[Broy]

Hi NY AINA HENINTSOA,

From what I understand, you are simulating a quadrotor in Simulink using a MATLAB S-Function, and you’re representing the attitude with a quaternion. Right now, you’re normalizing the quaternion in mdlOutputs, but the actual continuous state vector ‘x’ can still drift from unit length during numerical integration. Since this internal state feeds back into your dynamics, that drift can cause inaccuracies. So, your main concern is how to ensure the quaternion remains normalized after each solver integration step, especially since mdlUpdate only applies to discrete states.

There are two potential ways to enforce normalization in your S-function:

1. Normalize inside mdlDerivatives:

This ensures the input quaternion used to calculate the state derivative is always clean and normalized.

function dx = normAttitude(t, x, omega)
    % Normalize quaternion
    q = x(1:4) / norm(x(1:4));
    % ... rest of your quaternion kinematics and dynamics
    dx = ...;
end

2. Use a projection method:

This is the "cleanest" method according to Simulink’s design. You register a Projection callback in your Level-2 MATLAB S-function. The Simulink solver calls Projection after each integration step, allowing you to “project” states back onto meaningful constraints (Normalization).

function setup(block)
    % ... other setup code ...
    block.RegBlockMethod('Projection', @projStates);  
end

function projStates(block)
    % Normalize quaternion part of the state
    q = block.ContStates.Data(1:4);
    block.ContStates.Data(1:4) = q / norm(q);
end

Helpful Links and Resources you can refer to:

• https://www.mathworks.com/matlabcentral/answers/2173407-how-to-migrate-a-level-1-matlab-s-function-to-a-matlab-system-block-and-model-the-internal-states
• https://www.mathworks.com/help/simulink/sfg/projection.html

Hope this helps.

[NY AINA HENINTSOA]

Thank you so much for your answer. I'll apply what you advised and thanks for the resources.

Grateful for the help!