Merge branch 'butbetter' into 'main'

butbetter

See merge request acubesat/adcs/adcs-simulation-julia!4

Manifest.toml

@@ -2,7 +2,7 @@
 
 julia_version = "1.10.0-beta2"
 manifest_format = "2.0"
-project_hash = "3ef5a4355337de3e0a980ddcc7d8b1c53c65e283"
+project_hash = "d2153bff797d1f45fcfbad94a8711874cae1b019"
 
 [[deps.Accessors]]
 deps = ["CompositionsBase", "ConstructionBase", "Dates", "InverseFunctions", "LinearAlgebra", "MacroTools", "Requires", "Test"]
@@ -163,6 +163,11 @@ weakdeps = ["InverseFunctions"]
     [deps.CompositionsBase.extensions]
     CompositionsBaseInverseFunctionsExt = "InverseFunctions"
 
+[[deps.ConcreteStructs]]
+git-tree-sha1 = "f749037478283d372048690eb3b5f92a79432b34"
+uuid = "2569d6c7-a4a2-43d3-a901-331e8e4be471"
+version = "0.2.3"
+
 [[deps.ConcurrentUtilities]]
 deps = ["Serialization", "Sockets"]
 git-tree-sha1 = "5372dbbf8f0bdb8c700db5367132925c0771ef7e"

Project.toml

@@ -4,6 +4,7 @@ authors = ["Orestis Ousoultzoglou <[email protected]>", "Romanos Voulgar
 version = "0.0.1-DEV"
 
 [deps]
+ConcreteStructs = "2569d6c7-a4a2-43d3-a901-331e8e4be471"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Optim = "429524aa-4258-5aef-a3af-852621145aeb"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"

src/ADCSSims.jl

@@ -1,12 +1,14 @@
 module ADCSSims
 
+using ConcreteStructs
 using SatelliteDynamics
 using SatelliteToolboxGeomagneticField
 
 using LinearAlgebra
 using Plots
 using Statistics
 
+include("quaternion.jl")
 include("dynamics.jl")
 include("losses.jl")
 include("utils.jl")

src/dynamics.jl

@@ -1,33 +1,27 @@
-qderiv(w, q) = 0.5 * (quat_mult(vcat(0, w), q))
+qderiv(w, q) = 0.5 * (Quaternion([w; 0.0]) * q)
 
 function wqderiv(I, w, torque, q)
     dw_dt = vec(I \ (torque - w × (I * w)))
-
     dq_dt = qderiv(w, q)
-
     return dw_dt, dq_dt
 end
 
 function rk4(I, w, torque, q, dt)
     k1_w, k1_q = wqderiv(I, w, torque, q)
-    k2_w, k2_q = wqderiv(I, w .+ 0.5 .* dt .* k1_w, torque, q .+ 0.5 .* dt .* k1_q)
-    k3_w, k3_q = wqderiv(I, w .+ 0.5 .* dt .* k2_w, torque, q .+ 0.5 .* dt .* k2_q)
-    k4_w, k4_q = wqderiv(I, w .+ dt .* k3_w, torque, q .+ dt .* k3_q)
-
+    k2_w, k2_q = wqderiv(I, w .+ 0.5 .* dt .* k1_w, torque, q + 0.5 * dt * k1_q)
+    k3_w, k3_q = wqderiv(I, w .+ 0.5 .* dt .* k2_w, torque, q + 0.5 * dt * k2_q)
+    k4_w, k4_q = wqderiv(I, w .+ dt .* k3_w, torque, q + dt * k3_q)
     new_w = w .+ (dt / 6.0) .* (k1_w .+ 2 .* k2_w .+ 2 .* k3_w .+ k4_w)
-    new_q = q .+ (dt / 6.0) .* (k1_q .+ 2 .* k2_q .+ 2 .* k3_q .+ k4_q)
-
+    new_q = q + (dt / 6.0) * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
     new_q = normalize(new_q)
-
     return new_w, new_q
 end
 
 function rk4_filter(w, q, dt)
     k1_q = qderiv(w, q)
-    k2_q = qderiv(w, q .+ 0.5 .* dt .* k1_q)
-    k3_q = qderiv(w, q .+ 0.5 .* dt .* k2_q)
-    k4_q = qderiv(w, q .+ dt .* k3_q)
-    new_q = q .+ (dt / 6.0) .* (k1_q .+ 2 .* k2_q .+ 2 .* k3_q .+ k4_q)
-    new_q = normalize(new_q)
+    k2_q = qderiv(w, q + 0.5 * dt * k1_q)
+    k3_q = qderiv(w, q + 0.5 * dt * k2_q)
+    k4_q = qderiv(w, q + dt * k3_q)
+    new_q = normalize(q + (dt / 6.0) * (k1_q + 2 * k2_q + 2 * k3_q + k4_q))
     return new_q
 end

src/losses.jl

@@ -1 +1,6 @@
-mse(ŷ, y; agg = mean) = agg(abs2.(ŷ .- y))
+mse(ŷ, y; agg=mean) = agg(abs2.(ŷ .- y))
+
+function qloss(q̂, q)
+    relq = q * conj(q̂)
+    return norm(vector(relq))
+end

src/main.jl

@@ -8,7 +8,6 @@ sigma_v = 0.0026;
 mag_noise = 1e-3
 sun_noise = 1e-3
 dt = 0.1
-
 constant_params = ADCSSims.parameter_struct(inertia_matrix,
     sigma_u,
     sigma_v,
@@ -17,39 +16,42 @@ constant_params = ADCSSims.parameter_struct(inertia_matrix,
     dt)
 
 (q_history, w_history, bias_history, mag_eci, sun_eci, mag_noisy_history, sun_noisy_history, gyro_noisy_history) = ADCSSims.run_groundtruth_simulation(constant_params)
-gt_target = [q_history; bias_history]
-
 sun_noisy_history[:, 5001:6500] = zeros(3, 1500)
 
 function objective_function(x)
     tunable_params = ADCSSims.package_weights(x)
-    state_estimation_history = ADCSSims.run_filter_simulation(tunable_params,
+    statepred = ADCSSims.run_filter_simulation(tunable_params,
         constant_params,
         mag_noisy_history,
         sun_noisy_history,
         mag_eci,
         sun_eci,
         gyro_noisy_history)
-    return ADCSSims.mse(gt_target[1:4, :], state_estimation_history[1:4, :])
+
+    q̂ = [sp.q for sp in statepred]
+    return sum(ADCSSims.qloss.(q̂, q_history))
 end
 
 initial_x = [-6.0, -16, -6, -2]
-
 result = optimize(objective_function,
     initial_x,
     Optim.NelderMead(),
-    Optim.Options(iterations = 200, show_trace = true, g_tol = 1e-15))
+    Optim.Options(iterations=200, show_trace=true, g_tol=1e-15))
 
 optimized_x = Optim.minimizer(result)
-
 tunable_params = ADCSSims.package_weights(optimized_x)
-state_estimation_history = ADCSSims.run_filter_simulation(tunable_params,
+ŷ = ADCSSims.run_filter_simulation(tunable_params,
     constant_params,
     mag_noisy_history,
     sun_noisy_history,
     mag_eci,
     sun_eci,
     gyro_noisy_history)
 
-ADCSSims.plot_histories(gt_target, state_estimation_history)
-ADCSSims.plot_difference(gt_target, state_estimation_history)
+len = length(ŷ)
+y = Vector{ADCSSims.KFState}(undef, len)
+for i in 1:len
+    y[i] = ADCSSims.KFState(q_history[i], bias_history[:, i])
+end
+ADCSSims.plot_histories(ŷ, y)
+ADCSSims.plot_difference(y, ŷ)

src/mekf.jl

@@ -1,56 +1,57 @@
-mutable struct KalmanFilter
-    transition_fun::Function
-    transition_fun_jacobian::Function
-    Q::Matrix{Float64}
-    measurement_fun::Function
-    measurement_fun_jacobian::Function
-    R::Matrix{Float64}
-    dt::Float64
+@concrete struct KalmanFilter
+    transition_fun
+    transition_fun_jacobian
+    Q
+    measurement_fun
+    measurement_fun_jacobian
+    R
+    dt
 end
 
-function predict(state, P, kf::KalmanFilter, gyroscope_measurement)
-    bias = state[5:7]
-    q = state[1:4]
-    F_k = kf.transition_fun_jacobian(gyroscope_measurement, bias)
-    kf.transition_fun(q, gyroscope_measurement, bias, kf.dt)
-    new_state = kf.transition_fun(q, gyroscope_measurement, bias, kf.dt)
-    Phi = exp(F_k * kf.dt)
-    new_P = Phi * P * transpose(Phi) + kf.Q
+@concrete struct KFState
+    q
+    bias
+end
+
+function predict(state::KFState, P, KF::KalmanFilter, gyroscope_measurement)
+    bias = state.bias
+    F_k = KF.transition_fun_jacobian(gyroscope_measurement, bias)
+    new_state = KF.transition_fun(state, gyroscope_measurement, KF.dt)
+    Phi = exp(F_k * KF.dt)
+    new_P = Phi * P * transpose(Phi) + KF.Q
     return (new_state, new_P)
 end
 
-function update(state,
+function update(state::KFState,
     P,
-    kf::KalmanFilter,
+    KF::KalmanFilter,
     groundtruth_measurements::Tuple,
     reference_vectors::Tuple)
-    q = state[1:4]
+
+    q = state.q
     mag_eci = reference_vectors[1]
     sun_eci = reference_vectors[2]
-    H_k = kf.measurement_fun_jacobian(state[1:4], mag_eci, sun_eci)
-    mag_body, sun_body = kf.measurement_fun(q, mag_eci, sun_eci)
-    Kg = P * transpose(H_k) / (H_k * P * transpose(H_k) + kf.R)
-
+    H_k = KF.measurement_fun_jacobian(q, mag_eci, sun_eci)
+    mag_body, sun_body = KF.measurement_fun(q, mag_eci, sun_eci)
+    Kg = P * transpose(H_k) / (H_k * P * transpose(H_k) + KF.R)
     local_error_state = Kg * ([groundtruth_measurements[1]; groundtruth_measurements[2]] -
-                         [mag_body; sun_body])
-    local_error_quaternion = [1; 0.5 * local_error_state[1:3]]
-
-    new_q = quat_mult(state[1:4], local_error_quaternion)
-    new_q = new_q / norm(new_q)
-    new_bias = state[5:7] + local_error_state[4:6]
-
+                              [mag_body; sun_body])
+    local_error_quaternion = Quaternion([1; 0.5 * local_error_state[1:3]])
+    new_q = normalize(q * local_error_quaternion)
+    new_bias = state.bias + local_error_state[4:6]
     N_params = 6
     identity_matrix = 1.0 * I(N_params)
-
     new_P = (identity_matrix .- Kg * H_k) * P
-    new_state = [new_q; new_bias]
+    new_state = KFState(new_q, new_bias)
     return (new_state, new_P)
 end
 
-function transition_function(q, gyroscope_measurement, bias, dt)
+function transition_function(state::KFState, gyroscope_measurement, dt)
+    q = state.q
+    bias = state.bias
     w = gyroscope_measurement - bias
     q = rk4_filter(w, q, dt)
-    return vcat(q, bias)
+    return KFState(q, bias)
 end
 
 function transition_function_jacobian(gyroscope_measurement, bias)
@@ -59,8 +60,8 @@ function transition_function_jacobian(gyroscope_measurement, bias)
 end
 
 function measurement_function(q, mag_eci, sun_eci)
-    mag_body = rotate_vector_by_quaternion(mag_eci, q)
-    sun_body = rotate_vector_by_quaternion(sun_eci, q)
+    mag_body = rotvec(mag_eci, q)
+    sun_body = rotvec(sun_eci, q)
     return (mag_body, sun_body)
 end
 

src/noise_models.jl

@@ -4,8 +4,9 @@ function get_noisy_measurements(q, gt_angular_velocity, bias, mag_ref, sun_ref,
     total_noise = 0.5 .* (old_bias .+ bias) .+
                   sqrt((params.sigma_v^2 ./ params.dt .+
                         params.sigma_u^2 .* params.dt ./ 12)) .* randn(3)
+
     gyroscope_measurement = gt_angular_velocity + total_noise
-    mag_noisy = rotate_vector_by_quaternion(mag_ref, q) + params.mag_noise * randn(3)
-    sun_noisy = rotate_vector_by_quaternion(sun_ref, q) + params.sun_noise * randn(3)
+    mag_noisy = rotvec(mag_ref, q) + params.mag_noise * randn(3)
+    sun_noisy = rotvec(sun_ref, q) + params.sun_noise * randn(3)
     return (mag_noisy, sun_noisy, gyroscope_measurement, bias)
 end

src/parameters.jl

@@ -1,4 +1,4 @@
-mutable struct parameter_struct
+struct parameter_struct
     inertia_matrix::Matrix{Float64}
     sigma_u::Float64
     sigma_v::Float64

src/plots.jl

@@ -1,63 +1,40 @@
-function plot_histories(groundtruth_state_history, state_estimation_history)
-    p = plot(layout = (7, 1), size = (600, 1200))
-
+function plot_histories(ŷ, y)
+    get_comp(comp_func, array) = [comp_func(s) for s in array]
+    funcs = [
+        s -> s.q.q1,
+        s -> s.q.q2,
+        s -> s.q.q3,
+        s -> s.q.q4,
+        s -> s.bias[1],
+        s -> s.bias[2],
+        s -> s.bias[3]
+    ]
+    titles = ["q1 values", "q2 values", "q3 values", "q4 values", "bias1 values", "bias2 values", "bias3 values"]
+    p = plot(layout=(7, 1), size=(600, 1200))
     for i in 1:7
-        plot!(p[i],
-            groundtruth_state_history[i, :],
-            label = "Ground Truth",
-            xlabel = "Timestep",
-            ylabel = "Value",
-            linewidth = 2)
-        plot!(p[i],
-            state_estimation_history[i, :],
-            label = "Estimated",
-            xlabel = "Timestep",
-            ylabel = "Value",
-            linewidth = 2)
+        ŷ_vals = get_comp(funcs[i], ŷ)
+        y_vals = get_comp(funcs[i], y)
+        plot!(p[i], ŷ_vals, label="Estimated", xlabel="Timestep", ylabel="Value", linewidth=2, title=titles[i])
+        plot!(p[i], y_vals, label="Ground Truth", xlabel="Timestep", ylabel="Value", linewidth=2)
     end
-
-    display(current())
-end
-
-function quat_to_euler_deg(q::Array{Float64, 1})
-    qw, qx, qy, qz = q
-
-    roll = atan(2 * (qw * qx + qy * qz), 1 - 2 * (qx^2 + qy^2))
-    pitch = asin(2 * (qw * qy - qz * qx))
-    yaw = atan(2 * (qw * qz + qx * qy), 1 - 2 * (qy^2 + qz^2))
-
-    return [(roll * 180 / π), (pitch * 180 / π), (yaw * 180 / π)]
+    display(p)
 end
 
-function plot_difference(gt_target::Array{Float64, 2},
-    state_estimation_history::Array{Float64, 2})
-    if size(gt_target) != size(state_estimation_history)
-        println("Error: Dimensions of gt_target and state_estimation_history must match.")
-        return
-    end
-
-    nrows, ncols = size(gt_target)
-
-    difference_array = zeros(Float64, nrows, ncols)
-
-    for t in 1:ncols
-        q_gt = gt_target[1:4, t]
-        q_est = state_estimation_history[1:4, t]
-
-        q_gt ./= norm(q_gt)
-        q_est ./= norm(q_est)
-
-        q_rel = quat_mult(q_est, quaternion_conjugate(q_gt))
-
+function plot_difference(ŷ, y)
+    len = length(y)
+    difference_array = zeros(Float64, 6, len)
+    ŷqs = [s.q for s in ŷ]
+    yqs = [s.q for s in y]
+    for i in 1:len
+        q_rel = ŷqs[i] * conj(yqs[i])
         euler_angles_deg = quat_to_euler_deg(q_rel)
-
-        difference_array[1:3, t] = euler_angles_deg
+        difference_array[1:3, i] = euler_angles_deg
     end
 
-    difference_array[4:6, :] = gt_target[5:7, :] - state_estimation_history[5:7, :]
-
-    p = plot(layout = (6, 1), legend = false, size = (800, 1200))
-
+    ŷbias = reduce(hcat, [s.bias for s in ŷ])
+    ybias = reduce(hcat, [s.bias for s in y])
+    difference_array[4:6, :] = ybias - ŷbias
+    p = plot(layout=(6, 1), legend=false, size=(800, 1200))
     labels = [
         "Roll Difference",
         "Pitch Difference",
@@ -70,10 +47,9 @@ function plot_difference(gt_target::Array{Float64, 2},
     for i in 1:6
         plot!(p[i],
             difference_array[i, :],
-            title = labels[i],
-            xlabel = "Time",
-            ylabel = "Degrees")
+            title=labels[i],
+            xlabel="Time",
+            ylabel="Degrees")
     end
-
     display(p)
 end