Hi!
I am trying to model the attitude propagation with quaternions using ModelingToolkit.jl. I could make a very simple example as shown below:
using ModelingToolkit
using OrdinaryDiffEq
using ReferenceFrameRotations
using StaticArrays
function vectdquat(vq, w)
q = Quaternion(vq)
dq = dquat(q, w)
return @SVector [dq[1], dq[2], dq[3], dq[4]]
end
@variables t vq[1:4](t) RHS[1:4](t)
@parameters w[1:3]
@register vectdquat(vq, w)
D = Differential(t)
@named att_prop = ODESystem(
D.(vq) .~ vectdquat(vq, w),
)
prob = ODEProblem(
att_prop,
[vq[1] => 1.0, vq[2] => 0.0, vq[3] => 0.0, vq[4] => 0.0],
(0.0, 100.0),
[w[1] => 0.05, w[2] => 0.0, w[3] => 0.0]
)
sol = solve(prob, Tsit5())
Now, I need to add a constraint equation to force the quaternion to be in a unit sphere. It is something like vq .~ vq ./ norm(vq). I tried extending the ODESystem, using a vector of equations (which does not seem to support variables that are vectors), and any other method I could find from the documentation. However, I always get an error when building the ODESystem. Can anyone help me?
