work with help from matty

Quaternions.jl

@@ -29,7 +29,7 @@ end
 
 function QuaternionMultiplication(l::Quaternion, r::Quaternion)
     R = [
-        r.r r.k r.j r.i
+        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
@@ -45,7 +45,7 @@ Base.:*(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.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))
 

SADC.jl

@@ -1,23 +1,28 @@
 using LinearAlgebra
 using NumericalIntegration
-
+using ProgressBars
 
 
 include("Quaternions.jl")
 
 dt = 0.005
-time = 0.0:dt:100
+time = 0.0:dt:60
 
 I = [3 0 0; 0 4 0; 0 0 2]
+Iinv = inv(I)
 T = [0; 0; 0]
 ω = [2.001; 0.001; 0.001]
+ω′ = [0; 0; 0]
 
 q = Quaternion([0; 0; 0; 1])
 
 ω_last = [0; 0; 0; ω]
-ω_temp = similar(ω_last)
+ω0 = copy(ω_last)
+ω_area = [0; 0; 0; 0; 0; 0]
 
-integrate_ω(T, I, ω) = inv(I) * (T - cross(ω, I * ω))
+β = [0; 0; 0]
+
+integrate_ω(T, I, ω) = Iinv * (T - cross(ω, I * ω))
 
 function q_step(β)
     mag = β .^ 2 |> sum |> sqrt
@@ -34,40 +39,40 @@ function integrate_vector(v, v_last)
     return v_new
 end
 
-yaw = []
-pitch = []
-roll = []
-for t in time
-
 
+qs = []
+for t in ProgressBar(time)
+    # t = collect(time)[1]
     ω′ = integrate_ω(T, I, ω)
-    ω_new = integrate_vector([ω; ω′], ω_last)
-    ω = ω_new[4:6]
 
-    β = ω_new[1:3] .- ω
+
+    ω_new = integrate_vector([ω; ω′], ω_last)
+    β = ω_new[1:3] .- β
+
+    ω_last = [ω; ω′]
+    ω = ω_new[4:6] .- ω
+    ω_area = ω_new .+ ω_area
 
     q = q * q_step(β)
 
-    y, p, r = q_to_Euler(q)
-    push!(yaw, y)
-    push!(pitch, p)
-    push!(roll, r)
+    push!(qs, q)
 
 end
 
 
 
 # Plot 
-# using Plots
-# let
-#     plot(time, yaw, label = "Yaw")
-#     plot!(time, pitch, label = "Pitch")
-#     plot!(time, roll, label = "Roll")
-#     title!("Dancing T Handle")
-#     xlabel!("Seconds")
-#     ylabel!("Degrees")
-#     savefig("DancingT.png")
-# end
+using Plots
+let
+    plot(title = "T Handle Quaternion")
+    plot!([q[1] for q in qs], label = "i")
+    plot!([q[2] for q in qs], label = "j")
+    plot!([q[3] for q in qs], label = "k")
+    plot!([q[4] for q in qs], label = "r")
+end
+# println("done")
+
+# plot(qs)
 
 # Write to CSV
 # using CSV
@@ -83,3 +88,4 @@ end
 
 # CSV.write("ypr.csv", df, header = false)
 # df
+