Mekf

.gitignore

@@ -127,3 +127,9 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+# LaTex Build Files
+*.aux
+*.fdb_latexmk
+*.fls
+*.gz

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

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

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