Everything except integrators implemented for #3

Quaternions.jl

@@ -0,0 +1,54 @@
+
+
+export Quaternion
+
+struct Quaternion
+    i::Float64
+    j::Float64
+    k::Float64
+    r::Float64
+
+    Quaternion(i, j, k, r) =
+        norm([i, j, k, r]) ≈ 1 ? new(i, j, k, r) : error("Magnitude not equal to 1.")
+    Quaternion(q) = Quaternion(q[1], q[2], q[3], q[4])
+    Quaternion() = new(0, 0, 0, 1)
+
+    Quaternion(yaw, pitch, roll) =
+        Quaternion([0 0 sin(yaw / 2) cos(yaw / 2)]) *
+        Quaternion([0 sin(pitch / 2) 0 cos(pitch / 2)]) *
+        Quaternion([sin(roll / 2) 0 0 cos(roll / 2)])
+
+    function Quaternion(ē, ϕ)
+        if sum(ē) == 0
+            return Quaternion([0 0 0 cos(ϕ / 2)])
+        end
+        dir = normalize(ē) * sin(ϕ / 2)
+        return Quaternion(dir[1], dir[2], dir[3], cos(ϕ / 2))
+    end
+
+end
+
+function QuaternionMultiplication(l::Quaternion, r::Quaternion)
+    R = [
+        r.r r.k r.j r.i
+        -r.k r.r r.i r.j
+        r.j -r.i r.r r.k
+        -r.i -r.j -r.k r.r
+    ]
+    L = [l.i; l.j; l.k; l.r]
+    return Quaternion(R * L)
+
+end
+
+Base.:*(l::Quaternion, r::Quaternion) =
+    QuaternionMultiplication(l::Quaternion, r::Quaternion)
+
+Base.iterate(q::Quaternion, state = 1) =
+    state > 4 ? nothing : (collect(q)[state], state + 1)
+Base.length(q::Quaternion) = 4
+Base.collect(q::Quaternion) = [q.i q.j q.k q.r]
+Base.getindex(q::Quaternion, i) = collect(q)[i]
+Base.isapprox(a::Quaternion, b::Quaternion) = isapprox(collect(a), collect(b))
+
+LinearAlgebra.norm(q::Quaternion) = norm(collect(q))
+LinearAlgebra.normalize(q::Quaternion) = collect(q) / norm(q)

SADC.jl

@@ -0,0 +1,23 @@
+using LinearAlgebra
+
+include("Quaternions.jl")
+
+I = [3 0 0; 0 4 0; 0 0 2]
+T = [0; 0; 0]
+dt = 0.005
+ω₀ = [2.001; 0.001; 0.001]
+q₀ = [0; 0; 0; 1]
+
+
+integrate_ω(T, I, ω) = inv(I) * (T - cross(ω, I * ω))
+
+
+integrate_ω(T, I, ω₀)
+
+function q_step(β)
+    mag = β .^ 2 |> sum |> sqrt
+
+    Quaternion([normalize(β) .* sin(mag / 2); cos(mag / 2)])
+
+end
+