diff --git a/.gitignore b/.gitignore index b6e4761..3743b8b 100644 --- a/.gitignore +++ b/.gitignore @@ -127,3 +127,9 @@ dmypy.json # Pyre type checker .pyre/ + +# LaTex Build Files +*.aux +*.fdb_latexmk +*.fls +*.gz \ No newline at end of file diff --git a/Simulator/gyro.jl b/Simulator/gyro.jl new file mode 100644 index 0000000..105a870 --- /dev/null +++ b/Simulator/gyro.jl @@ -0,0 +1,20 @@ +using Random, Distributions + +mutable struct Gyro + β::Vector{Float64} + Vβ::Matrix{Float64} + Vω::Matrix{Float64} +end + +""" update(g, ω) + Arguments: + - g: The gyro | Gyro + - ω: True angular velocity | [3,] + Returns: + - ω: Measured angular velocity | [3,] + +""" +function update(g::Gyro, ω::Vector{Float64}) + g.β += rand(MvNormal([0; 0; 0], g.Vβ)) + return ω + g.β + rand(MvNormal([0; 0; 0], g.Vω)) +end \ No newline at end of file diff --git a/Simulator/kf.jl b/Simulator/kf.jl new file mode 100644 index 0000000..993fca2 --- /dev/null +++ b/Simulator/kf.jl @@ -0,0 +1,128 @@ +# [Simulator/kf.jl] + +using LinearAlgebra +include("quaternions.jl") + +mutable struct EKF + q::Vector{Float64} + β::Vector{Float64} + P::Matrix{Float64} +end + +function f( + q::Vector{Float64}, + β::Vector{Float64}, + ω::Vector{Float64}, + δt::Float64 +) + θ = norm(ω - β) * δt + r = normalize(ω - β) + return L(q) * [cos(θ / 2); r * sin(θ / 2)] +end + +function step( + e::EKF, + ω::Vector{Float64}, + δt::Float64, + ⁿr_mag::Vector{Float64}, + ⁿr_sun::Vector{Float64}, + ᵇr_mag::Vector{Float64}, + ᵇr_sun::Vector{Float64}, + logging::Bool = false, +) + if logging + println("Before: ") + println("e.q: ", e.q) + println("e.β: ", e.β) + println("e.P: ", e.P) + println("ω: ", ω) + println("δt: ", δt) + println("ⁿr_mag: ", ⁿr_mag) + println("ⁿr_sun: ", ⁿr_sun) + println("ᵇr_mag: ", ᵇr_mag) + println("ᵇr_sun: ", ᵇr_sun) + end + q = e.q + β = e.β + P = e.P + W = I(6) * 1e-6 # related to some noise or something (ask Zac) + V = I(6) * 1e-6 # some other noise + # Predict: + qₚ = f(q, β, ω, δt) # β remains constant + # The following is equivalent to: + # R = exp(-hat(ω - β) * δt) + # By the https://en.wikipedia.org/wiki/Rodrigues'_rotation_formula + v = - (ω - β) + mag = norm(v) + v̂ = hat(v/mag) + R = I(3) + (v̂) * sin(mag * δt) + (v̂*v̂) * (1 - cos(mag * δt)) # equivalent to e^{-hat(ω - β) * δt} + A = [ + R (-δt*I(3)) + zeros(3, 3) I(3) + ] + Pₚ = A * P * A' + W + + # Innovation + Q = quaternionToMatrix(qₚ)' + Z = [ᵇr_mag; ᵇr_sun] - [Q zeros(3, 3); zeros(3, 3) Q] * [ⁿr_mag; ⁿr_sun] + C = [hat(ᵇr_mag) zeros(3, 3); hat(ᵇr_sun) zeros(3, 3)] + S = C * Pₚ * C' + V + + # Kalman Gain + L = Pₚ * C' * inv(S) + + # Update + δx = L * Z + ϕ = δx[1:3] + δβ = δx[4:6] + θ = norm(ϕ) + r = normalize(ϕ) + qᵤ = ⊙(qₚ, [cos(θ / 2); r * sin(θ / 2)]) + βᵤ = e.β + δβ + Pᵤ = (I(6) - L * C) * Pₚ * (I(6) - L * C)' + L * V * L' + + e.q = qᵤ + e.β = βᵤ + e.P = Pᵤ + if logging + println("After: ") + println("e.q: ", e.q) + println("e.β: ", e.β) + println("e.P: ", e.P) + end + return e +end + +# for the "simplest MEKF" +function simplest_step( + e::EKF, + ω::Vector{Float64}, + δt::Float64, + ⁿr_mag::Vector{Float64}, + ⁿr_sun::Vector{Float64}, + ᵇr_mag::Vector{Float64}, + ᵇr_sun::Vector{Float64} +) + q = e.q + P = e.P + W = I(3) * 1e-6 + V = I(6) * 1e-6 + # Predict + r = normalize(ω) + θ = norm(ω) * δt + qₚ = ⊙(q, [cos(θ / 2); r * sin(θ / 2)]) + Pₚ = P + W + # Innovation + Q = quaternionToMatrix(qₚ)' + Z = [ᵇr_mag; ᵇr_sun] - [Q zeros(3, 3); zeros(3, 3) Q] * [ⁿr_mag; ⁿr_sun] + C = [hat(Q * ⁿr_mag); hat(Q * ⁿr_sun)] + S = C * Pₚ * C' + V + # Kalman Gain + L = Pₚ * C' * inv(S) + # Update + ϕ = L * Z + θ = norm(ϕ) + r = normalize(ϕ) + e.q = ⊙(qₚ, [cos(θ / 2); r * sin(θ / 2)]) + e.P = (I(3) - L * C) * Pₚ * (I(3) - L * C)' + L * V * L' +end \ No newline at end of file diff --git a/Simulator/mocksat.js b/Simulator/mocksat.js deleted file mode 100644 index 14ecd39..0000000 --- a/Simulator/mocksat.js +++ /dev/null @@ -1,35 +0,0 @@ -const readline = require('readline') - -// while(true){ -// console.log('hello world') -// } -console.log('hello world, from mocksatjs') - -let ω = [] -let b = [] - -const rl = readline.createInterface({ - input: process.stdin, - output: process.stdout -}) -rl.on('line', input => { - if (input.length < 4 || input.slice(0, 3) != ">>>") return - input = input.slice(3) - let key = input[0] - let value = input.slice(1) - if (key == 'ω') { - ω = JSON.parse(value) - } else if (key == 'b') { - b = JSON.parse(value) - } else if (key == '?') { - let control = ω.map(v => -v * 0.001) - control = JSON.stringify(control) - console.log(`>>>M${control}`) - } - // console.log(ω, b) - // console.log(key, value) -}) - -rl.on('close', e => { - console.log(`eee${e}`) -}) diff --git a/Simulator/quaternions.jl b/Simulator/quaternions.jl index e12ea3b..2e9eeb5 100644 --- a/Simulator/quaternions.jl +++ b/Simulator/quaternions.jl @@ -17,12 +17,12 @@ using LinearAlgebra # For I, norm Returns: - M: A [3 × 3] skew-symmetric matrix | [3, 3] """ -function hat(v) - M = [0.0 -v[3] v[2]; - v[3] 0.0 -v[1]; - -v[2] v[1] 0.0] +function hat(v) + M = [0.0 -v[3] v[2] + v[3] 0.0 -v[1] + -v[2] v[1] 0.0] - return M + return M end """ L(q) @@ -36,11 +36,11 @@ end Returns: - M: Left-side matrix representing the given quaternion | [4, 4] """ -function L(q) +function L(q) qₛ, qᵥ = q[1], q[2:4] - M = [qₛ -qᵥ'; - qᵥ qₛ*I(3) + hat(qᵥ)] + M = [qₛ -qᵥ' + qᵥ qₛ*I(3)+hat(qᵥ)] return M end @@ -57,11 +57,11 @@ end Returns: - M: Right-side matrix representing the given quaternion | [4, 4] """ -function R(q) +function R(q) qₛ, qᵥ = q[1], q[2:4] - M = [qₛ -qᵥ'; - qᵥ qₛ*I(3) - hat(qᵥ)] + M = [qₛ -qᵥ' + qᵥ qₛ*I(3)-hat(qᵥ)] return M end @@ -77,7 +77,7 @@ end Returns: - q̇: Derivative of attitude, parameterized as a quaternion | [4,] """ -function qdot(q, ω) +function qdot(q, ω) q̇ = 0.5 * L(q) * H * ω return q̇ end @@ -87,5 +87,24 @@ const H = [zeros(1, 3); I(3)] # Converts from a 3-element vector to a 4-elem const T = [1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 -1] # Forms the conjugate of q, i.e. q† = Tq -⊙(q₁, q₂) = L(q₂) * q₁ # Hamiltonian product +⊙(q₁, q₂) = L(q₁) * q₂ # Hamiltonian product + +""" quaternionToMatrix (q) + Arguments: + - q: Scalar-first unit quaternion | [4,] + + Returns: + - Q: Rotation matrix representing the same rotation +""" +function quaternionToMatrix(q::Vector{Float64}) + s, v = q[1], q[2:4] + return I(3) + 2 * hat(v) * (s * I(3) + hat(v)) +end + +function qErr(q₁, q₂) + return norm((L(q₁)'*q₂)[2:4]) +end +function eulerError(e1, e2) + return acos(dot(e1, e2) / (norm(e1) * norm(e2))) +end \ No newline at end of file diff --git a/Simulator/simulator.jl b/Simulator/simulator.jl index 71400ed..5248d6d 100644 --- a/Simulator/simulator.jl +++ b/Simulator/simulator.jl @@ -6,15 +6,19 @@ """ module Simulator - using SatelliteDynamics, LinearAlgebra + using SatelliteDynamics, LinearAlgebra include("mag_field.jl") include("quaternions.jl") + include("kf.jl") + include("gyro.jl") + include("utils.jl") export initialize_orbit, intialize_orbit_oe # Generate valid initial conditions for a satellite export rk4 # Interface function for updating state export IGRF13 # Function call that provides magnetic field vector given position and time export my_sim + export my_sim_kf """ @@ -43,7 +47,7 @@ module Simulator r, v, q, ω = x[1:3], x[4:6], x[7:10], x[11:13] if norm(r) < Rₑ - error("Error: Satellite impacted Earth at time $t!") + error("Error: Satellite impacted Earth at time $t !") end @@ -240,19 +244,97 @@ module Simulator for i = 1:N - 1 r, v, q, ω = x[1:3], x[4:6], x[7:10], x[11:13] b = IGRF13(r, t) - x = rk4(x, J, control_fn(ω, b), t, dt) + m = control_fn(ω, b) + x = rk4(x, J, cross(m, b), t, dt) t += dt # Don't forget to update time (not that it really matters...) # q_hist[i + 1, :] .= x[7:10] q_hist[i + 1, 1:3] .= x[11:13] ω = norm(x[11:13]) + q_hist[i + 1, 4] = ω if ω < 0.001 q_hist = q_hist[1:i, :] break end - q_hist[i + 1, 4] = ω end return q_hist end + + function my_sim_kf(control_fn, N) + x₀ = initialize_orbit() + println("intialized orbit!") + + J = [0.3 0 0; 0 0.3 0; 0 0 0.3] # Arbitrary inertia matrix for the Satellite + t = Epoch(2020, 11, 30) # Starting time is Nov 30, 2020 + dt = 0.01 # Time step, in seconds + + q_true = zeros(N, 4) + q_predicted = zeros(N, 4) + ω_true = zeros(N, 1) + q_error = zeros(N, 1) + β_error = zeros(N, 1) + + x = x₀ + + q₀ = x₀[7:10] + β₀ = [0; 0; 0] + + P₀ = I(6) + println("q0:", q₀) + kf = EKF(q₀, β₀, P₀) + + Vβ = I(3) * 0.01 + Vω = I(3) * 0.1 + gyro = Gyro(β₀, Vβ, Vω) + + progress_step = 0.001 + cur = progress_step + + for i = 1:N - 1 + if i/N >= cur + println(string(cur * 100) * "%") + cur += progress_step + end + + r, v, q, ω = x[1:3], x[4:6], x[7:10], x[11:13] + ω_read = update(gyro, ω) + + b = IGRF13(r, t) + + r_sun = sun_position(t) + inertial_sun = normalize(r_sun - r) + inertial_mag = normalize(b) + + rsun = [0;0;0] + rmag = [0;0;0] + Q = quaternionToMatrix(q)' + body_sun = randomMatrix(0.001) * Q * (normalize(inertial_sun + rsun)) + body_mag = randomMatrix(0.001) * Q * (normalize(inertial_mag + rmag)) + step(kf, ω_read, dt, inertial_mag, inertial_sun, body_mag, body_sun) + q_true[i,:] .= q + q_predicted[i,:] .= kf.q + ω_true[i] = norm(ω) + q_error[i] = qErr(kf.q, q) + β_error[i] = eulerError(gyro.β, kf.β) + + + if norm(ω) < 0.01 + q_true = q_true[1:i, :] + q_predicted = q_predicted[1:i, :] + ω_true = ω_true[1:i] + q_error = q_error[1:i] + β_error = β_error[1:i] + break + end + # propogate everything forward + control, dt = control_fn(ω_read - kf.β, b) + dt /= 1e9 # convert from nanoseconds to seconds + x = rk4(x, J, cross(control, b), t, dt) + t += dt # Don't forget to update time + end + + return q_true, q_predicted, ω_true, q_error, β_error + + end end \ No newline at end of file diff --git a/Simulator/tests/tests.jl b/Simulator/tests/tests.jl index bb26472..48c4474 100644 --- a/Simulator/tests/tests.jl +++ b/Simulator/tests/tests.jl @@ -1,6 +1,6 @@ # [Simulator/test/tests.jl] -using Test, LinearAlgebra, SatelliteDynamics, Plots, JSON +using Test, LinearAlgebra, SatelliteDynamics, Plots, JSON, Random, Dates include("../simulator.jl"); using .Simulator @@ -106,16 +106,58 @@ end display(plot(e_hist, title ="Energy")) end +@testset "MEKF/DeTumbling" begin + Random.seed!() + function control_law(ω, b) + b̂ = b / norm(b) + k = 7e-4 + M = -k * (I(3) - b̂*b̂')*ω + m = 1 / (dot(b, b)) * cross(b, M) + return m, 1e8 # half a second in nano seconds + end + + q_true, q_predicted, ω_true, q_error, β_error = my_sim_kf(control_law, 100000) + name = "MEKF/DeTumbling" + q_plot = (plot( + hcat(q_true, q_predicted), + title=name, + xlabel="Time (s)", + ylabel="Angular Velocity (rad/s)", + labels=["s" "v1" "v2" "v3" "s'" "v1'" "v2'" "v3'"], + linecolor=[:blue :green :purple :orange :blue :green :purple :orange], + linewidth=[1 1 1 1 1 1 1 1], + )) + q_error_plot = plot( + q_error, + title="Quaternion Error", + xlabel="Time (s)", + ylabel="Radians", + ) + ω_plot = plot( + ω_true, + title="Angular Velocity", + xlabel="Time (s)", + ylabel="Radians/s" + ) + β_plot = plot( + β_error, + title="Gyro Bias Error", + xlabel="Time (s)", + ylabel="Radians" + ) + display(plot(q_plot, q_error_plot, ω_plot, β_plot, layout=(2,2), dpi=300)) + +end + @testset "DeTumbling" begin + println("DeTumbling") function control_law(ω, b) b̂ = b / norm(b) k = 7e-4 - # M = -k * (identity(3) - dot(b̂,b̂))*ω M = -k * (I(3) - b̂*b̂')*ω - # print(M, ω, b) - # print(dot(ω,ω),'\n') - return M + m = 1 / (dot(b, b)) * cross(b, M) + return m end data = my_sim(control_law) @@ -125,11 +167,12 @@ end @testset "DeTumbleIO" begin + Random.seed!() println("DeTumbleIO") inp = Pipe() out = Pipe() err = Pipe() - proc = run(Cmd(`python3 -u main.py`, dir = "/home/thetazero/Documents/pycubed/software_example_beepsat/state_machine/build/" ), inp, out, err, wait = false) + proc = run(Cmd(`faketime -f '-0 x100' python3 -u main.py`, dir = "/home/thetazero/Documents/pycubed/software_example_beepsat/state_machine/build/" ), inp, out, err, wait = false) close(out.in) close(err.in) Base.start_reading(out.out) @@ -139,29 +182,71 @@ end i=0 + last_time = time() * Int(1e9) # Time in nanoseconds function control_law(ω, b) - println("wrote to sensors") control = [0,0,0] - while true - write(inp, ">>>ω"*string(ω)*"\n") - write(inp, ">>>b"*string(b)*"\n") - write(inp, ">>>?\n") + got_control = false + got_time = false + + cur_time = 0 + write(inp, ">>>ω"*string(ω)*"\n") + write(inp, ">>>b"*string(b)*"\n") + while !(got_control && got_time) input = readline(proc.out) - println(input) + # println(input) if length(input) >= 4 && input[1:3] == ">>>" - if input[4] == 'M' + if input[4] == 'm' control = (input[5:length(input)]) control = JSON.parse(control) + got_control = true + elseif input[4] == 't' + cur_time = parse(Int64, input[5:length(input)]) + got_time = true end + end + if got_control && got_time break end end i+=1 - println(i) - return control + + # println("I: ", i) + # println("==========last_time: ", last_time, " cur_time: ", cur_time, " dt: ", cur_time-last_time) + dt = norm(cur_time - last_time) + last_time = cur_time + # println("control: ", control, " dt: ", dt/1e9) + return control, dt end - data = my_sim(control_law) - display(plot(data, title="DeTumbleIO", xlabel="Time (s)", ylabel="Angular Velocity (rad/s)")) + q_true, q_predicted, ω_true, q_error, β_error = my_sim_kf(control_law, 50000) + name = "MEKF/DeTumbling(IO)" + q_plot = (plot( + hcat(q_true, q_predicted), + title=name, + xlabel="Time (s)", + ylabel="Angular Velocity (rad/s)", + labels=["s" "v1" "v2" "v3" "s'" "v1'" "v2'" "v3'"], + linecolor=[:blue :green :purple :orange :blue :green :purple :orange], + linewidth=[1 1 1 1 1 1 1 1], + )) + q_error_plot = plot( + q_error, + title="Quaternion Error", + xlabel="Time (s)", + ylabel="Radians", + ) + ω_plot = plot( + ω_true, + title="Angular Velocity", + xlabel="Time (s)", + ylabel="Radians/s" + ) + β_plot = plot( + β_error, + title="Gyro Bias Error", + xlabel="Time (s)", + ylabel="Radians" + ) + display(plot(q_plot, q_error_plot, ω_plot, β_plot, layout=(2,2), dpi=300)) end diff --git a/Simulator/utils.jl b/Simulator/utils.jl new file mode 100644 index 0000000..524e74f --- /dev/null +++ b/Simulator/utils.jl @@ -0,0 +1,4 @@ +function randomMatrix(covariance) + ϕ = √covariance * randn(3) + return exp(hat(ϕ)) +end \ No newline at end of file diff --git a/summary/main.pdf b/summary/main.pdf new file mode 100644 index 0000000..3e72e95 Binary files /dev/null and b/summary/main.pdf differ diff --git a/summary/main.tex b/summary/main.tex new file mode 100644 index 0000000..8aa501e --- /dev/null +++ b/summary/main.tex @@ -0,0 +1,23 @@ +\documentclass{article} +\usepackage[utf8]{inputenc} +\usepackage{amsmath} +\usepackage{bm} +\usepackage{amsfonts} + +\setlength{\parindent}{0pt} + +\begin{document} + + \section{Results of Hughes, Peter - Spacecraft Attitude Dynamics} + + + $\bm{M}$ is torque, $\bm{m}$ is commanded magnetic dipole moment vector generated by the coils, and $\bm{b}$ is the local geomagnetic field. + \[\bm{M}=\bm{m}\times \bm{b}\] + + $\hat{\bm{b}}=\bm{b}/||\bm{b}||$ is the unit vector parallel to the local geomagnetic vector. + + \[\bm{M} = -k_{\omega}\left(\mathbb{I}_3 - \hat{\bm{b}}\hat{\bm{b}}^T\right)\bm{\omega}\] + + Since we can only control $\bm{m}$ we set $\bm{m}$ to be + \[\bm{m}=\frac{1}{||\bm{b}||^2}(\bm{b}\times \bm{M})\] +\end{document} \ No newline at end of file