Attitude Dynamics
Quaternion kinematics, and dynamics!
Overview
The Attitude model assumes a spacecraft with some orientation described by a scalar-last quaternion, and body rates which are small enough such that they appear constant for small numerical integration tolerance values.
You should normalize the quaternion vector at each time step using a ManifoldCallback or DiscreteCallback when simulating this model! Without normalizing, the solution will drift such that the quaternion state vector is no longer a unit quaternion. The dynamics in this model assume a unit quaternion norm!
\[\begin{aligned} \dot{q} &= \frac{1}{2} \begin{bmatrix} 0 && \omega_3 && -\omega_2 && \omega_1 \\ -\omega_3 && 0 && \omega_1 && \omega_2 \\ \omega_2 && -\omega_1 && 0 && \omega_3 \\ -\omega_1 && -\omega_2 && -\omega_3 && 0 \end{bmatrix} q \\ \dot{\omega} &= -J^{-1} (\omega\times) J \omega + J^{-1} L + u \\ \end{aligned}\]
Examples
julia> model = AttitudeSystem()Model Attitude with 7 equations Unknowns (7): (q(t))[1]: scalar-last attitude quaternion (q(t))[2]: scalar-last attitude quaternion (q(t))[3]: scalar-last attitude quaternion (q(t))[4]: scalar-last attitude quaternion ⋮ Parameters (15): J[1, 1]: moment of inertial matrix J[2, 1]: moment of inertial matrix J[3, 1]: moment of inertial matrix J[1, 2]: moment of inertial matrix ⋮
Let's compute the Jacobian for these dynamics.
julia> J = calculate_jacobian(AttitudeSystem())7×7 Matrix{Num}: 0 … (1//2)*(q(t))[2] (-1//2)*(ω(t))[3] (-1//2)*(q(t))[1] (1//2)*(ω(t))[2] (1//2)*(q(t))[4] (-1//2)*(ω(t))[1] (-1//2)*(q(t))[3] 0 ((J[1, 2]*J[2, 3] - J[1, 3]*J[2, 2])*(J[1, 3]*(ω(t))[2] - J[2, 3]*(ω(t))[1])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) + ((-J[1, 1]*(ω(t))[1] - J[1, 2]*(ω(t))[2] - 2J[1, 3]*(ω(t))[3] + J[3, 3]*(ω(t))[1])*(-J[1, 2]*J[3, 3] + J[1, 3]*J[3, 2])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) + ((J[2, 1]*(ω(t))[1] + J[2, 2]*(ω(t))[2] + 2J[2, 3]*(ω(t))[3] - J[3, 3]*(ω(t))[2])*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) 0 … ((-J[1, 1]*J[2, 3] + J[1, 3]*J[2, 1])*(J[1, 3]*(ω(t))[2] - J[2, 3]*(ω(t))[1])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) + ((-J[2, 1]*J[3, 3] + J[2, 3]*J[3, 1])*(J[2, 1]*(ω(t))[1] + J[2, 2]*(ω(t))[2] + 2J[2, 3]*(ω(t))[3] - J[3, 3]*(ω(t))[2])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) + ((J[1, 1]*J[3, 3] - J[1, 3]*J[3, 1])*(-J[1, 1]*(ω(t))[1] - J[1, 2]*(ω(t))[2] - 2J[1, 3]*(ω(t))[3] + J[3, 3]*(ω(t))[1])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) 0 ((J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])*(J[2, 1]*(ω(t))[1] + J[2, 2]*(ω(t))[2] + 2J[2, 3]*(ω(t))[3] - J[3, 3]*(ω(t))[2])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) + ((J[1, 1]*J[2, 2] - J[1, 2]*J[2, 1])*(J[1, 3]*(ω(t))[2] - J[2, 3]*(ω(t))[1])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1])) + ((-J[1, 1]*J[3, 2] + J[1, 2]*J[3, 1])*(-J[1, 1]*(ω(t))[1] - J[1, 2]*(ω(t))[2] - 2J[1, 3]*(ω(t))[3] + J[3, 3]*(ω(t))[1])) / (J[1, 1]*(J[2, 2]*J[3, 3] - J[2, 3]*J[3, 2]) - J[1, 2]*(J[2, 1]*J[3, 3] - J[2, 3]*J[3, 1]) + J[1, 3]*(J[2, 1]*J[3, 2] - J[2, 2]*J[3, 1]))
Finally, let's construct a Julia function which implements these dynamics!
julia> f = AttitudeFunction()(::ODEFunction{true, SciMLBase.FullSpecialize, ModelingToolkit.var"#f#684"{RuntimeGeneratedFunctions.RuntimeGeneratedFunction{(:ˍ₋arg1, :ˍ₋arg2, :t), ModelingToolkit.var"#_RGF_ModTag", ModelingToolkit.var"#_RGF_ModTag", (0xf91961de, 0xdf6ab5a8, 0x77b4553c, 0xdfb450c0, 0x63a118d9), Nothing}, RuntimeGeneratedFunctions.RuntimeGeneratedFunction{(:ˍ₋out, :ˍ₋arg1, :ˍ₋arg2, :t), ModelingToolkit.var"#_RGF_ModTag", ModelingToolkit.var"#_RGF_ModTag", (0x29774b05, 0xa3d3e990, 0x17ffba35, 0xa8be0f76, 0xd50f7584), Nothing}}, LinearAlgebra.UniformScaling{Bool}, Nothing, Nothing, ModelingToolkit.var"#_jac#689"{RuntimeGeneratedFunctions.RuntimeGeneratedFunction{(:ˍ₋arg1, :ˍ₋arg2, :t), ModelingToolkit.var"#_RGF_ModTag", ModelingToolkit.var"#_RGF_ModTag", (0x3d9e9e19, 0xb3f0c85a, 0x1ba51c6d, 0x7eb95e6d, 0x7b8054e4), Nothing}, RuntimeGeneratedFunctions.RuntimeGeneratedFunction{(:ˍ₋out, :ˍ₋arg1, :ˍ₋arg2, :t), ModelingToolkit.var"#_RGF_ModTag", ModelingToolkit.var"#_RGF_ModTag", (0x24302948, 0xefbe1f48, 0xbcfac144, 0x75faadad, 0xf98f2eab), Nothing}}, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, ModelingToolkit.var"#51211#generated_observed#693"{Bool, ODESystem, Dict{Any, Any}, Vector{SymbolicUtils.BasicSymbolic{Real}}}, Nothing, ODESystem, Nothing, Nothing}) (generic function with 1 method)julia> let u = randn(7), p = randn(15), t = 0 f(u, p, t) end7-element Vector{Float64}: -0.4041877770735503 -0.2256878035538583 -1.0735161830035853 0.10736661610949678 -4.430854270591023 -2.417834444871205 -1.0881590723427366