mirror of
https://gitlab.com/MisterBiggs/SADC.jl.git
synced 2025-07-27 16:41:32 +00:00
92 lines
1.5 KiB
Julia
92 lines
1.5 KiB
Julia
using LinearAlgebra
|
|
using NumericalIntegration
|
|
using ProgressBars
|
|
|
|
|
|
include("Quaternions.jl")
|
|
|
|
dt = 0.005
|
|
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; ω]
|
|
ω0 = copy(ω_last)
|
|
ω_area = [0; 0; 0; 0; 0; 0]
|
|
|
|
β = [0; 0; 0]
|
|
|
|
integrate_ω(T, I, ω) = Iinv * (T - cross(ω, I * ω))
|
|
|
|
function q_step(β)
|
|
mag = β .^ 2 |> sum |> sqrt
|
|
|
|
Quaternion([normalize(β) .* sin(mag / 2); cos(mag / 2)])
|
|
|
|
end
|
|
|
|
function integrate_vector(v, v_last)
|
|
v_new = similar(v)
|
|
for i = 1:length(v)
|
|
v_new[i] = integrate([0, dt], [v_last[i], v[i]])
|
|
end
|
|
return v_new
|
|
end
|
|
|
|
|
|
qs = []
|
|
for t in ProgressBar(time)
|
|
# t = collect(time)[1]
|
|
ω′ = integrate_ω(T, I, ω)
|
|
|
|
|
|
ω_new = integrate_vector([ω; ω′], ω_last)
|
|
β = ω_new[1:3] .- β
|
|
|
|
ω_last = [ω; ω′]
|
|
ω = ω_new[4:6] .- ω
|
|
ω_area = ω_new .+ ω_area
|
|
|
|
q = q * q_step(β)
|
|
|
|
push!(qs, q)
|
|
|
|
end
|
|
|
|
|
|
|
|
# Plot
|
|
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
|
|
# using DataFrames
|
|
|
|
# ts = 100
|
|
# df = DataFrame(
|
|
# time = time[1:ts:end],
|
|
# yaw = yaw[1:ts:end],
|
|
# roll = roll[1:ts:end],
|
|
# pitch = pitch[1:ts:end],
|
|
# )
|
|
|
|
# CSV.write("ypr.csv", df, header = false)
|
|
# df
|
|
|