# How can I implement the TRIAD algorithm in Julia

**URL:** <https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151>\
**Category:** General Usage\
**Tags:** question, aerospace, rotations\
**Created:** [October 10, 2024, 2:20pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151 "2024-10-10T14:20:16Z")\
**Posts on this page:** 13\
**Page:** 2

<div class="post-metadata">

**Author:** ![p\_f](https://avatars.discourse-cdn.com/v4/letter/p/45deac/32.png) [@p\_f](https://discourse.julialang.org/u/p_f)\
**Post date:** [October 10, 2024, 5:01pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/21 "2024-10-10T17:01:14Z")

</div>

It can be simplified to:

```julia
function is_right_handed_orthonormal(x, y, z)
  R = [x y z]
  R*R' ≈ I && det(R) ≈ 1
end

```

which I think is better because `x ≈ 0` returns false unless exactly 0

edit: for example your function returns false for a rotation matrix I just constructed using euler angles

---

<div class="post-metadata">

**Author:** ![stevengj](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/stevengj/32/71_2.png) [@stevengj](https://discourse.julialang.org/u/stevengj)\
**Post date:** [October 10, 2024, 5:03pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/22 "2024-10-10T17:03:21Z")

</div>

> [@ufechner7](#):
>
> `(x ⋅ y) ≈ 0`

This is probably not what you want: it is equivalent to `(x ⋅ y) == 0` because you’re not providing any scale to compare against.

Since you checked that the norms are almost 1, your scale is one, so it would be sensible to check `abs(x ⋅ y) < ε` for some `ε`.

_Update:_ @p_f’s suggestion is much nicer.

> [@kellertuer](#):
>
> I think replacing the `det` with the dot/cross one might be a bit more stable

I have no idea what this sentence means?

I would tend to use StaticArrays.jl for 3d vectors like this, however.

---

<div class="post-metadata">

**Author:** ![ufechner7](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/ufechner7/32/51363_2.png) [@ufechner7](https://discourse.julialang.org/u/ufechner7)\
**Post date:** [October 10, 2024, 5:14pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/23 "2024-10-10T17:14:53Z")

</div>

> [@stevengj](#):
>
> I would tend to use [StaticArrays.jl](https://juliahub.com/ui/Packages/General/StaticArrays) for 3d vectors like this

I am using only SVectors in my code, but must this function also be modified to use StaticArrays?

```julia
function is_right_handed_orthonormal(x, y, z)
    R = [x y z]
    R*R' ≈ I && det(R) ≈ 1
end

```

---

<div class="post-metadata">

**Author:** ![stevengj](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/stevengj/32/71_2.png) [@stevengj](https://discourse.julialang.org/u/stevengj)\
**Post date:** [October 10, 2024, 5:28pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/24 "2024-10-10T17:28:50Z")

</div>

> [@ufechner7](#):
>
> but must this function also be modified to use StaticArrays?

No, it should be fine. `[x y z]` should construct an `SMatrix` from `SVectors`, and `R*R' ≈ I && det(R) ≈ 1` should be non-allocating for `SMatrix`.

---

<div class="post-metadata">

**Author:** ![ufechner7](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/ufechner7/32/51363_2.png) [@ufechner7](https://discourse.julialang.org/u/ufechner7)\
**Post date:** [October 10, 2024, 5:31pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/25 "2024-10-10T17:31:11Z")

</div>

So for now this is the solution:

```julia
function is_right_handed_orthonormal(x, y, z)
    R = [x y z]
    R*R' ≈ I && det(R) ≈ 1
end

"""
    rot3d(ax, ay, az, bx, by, bz)

Calculate the rotation matrix that needs to be applied on the reference frame (ax, ay, az) to match 
the reference frame (bx, by, bz).
All parameters must be 3-element vectors. Both refrence frames must be orthogonal,
all vectors must already be normalized.

Source: [TRIAD_Algorithm](http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm)
"""
function rot3d(ax, ay, az, bx, by, bz)
    @assert is_right_handed_orthonormal(ax, ay, az)
    @assert is_right_handed_orthonormal(bx, by, bz)
    R_ai = [ax az ay]
    R_bi = [bx bz by]
    return R_bi * R_ai'
end

```

---

<div class="post-metadata">

**Author:** ![stevengj](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/stevengj/32/71_2.png) [@stevengj](https://discourse.julialang.org/u/stevengj)\
**Post date:** [October 10, 2024, 5:32pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/26 "2024-10-10T17:32:30Z")

</div>

> [@ufechner7](#):
>
> So for now this is the solution:

The other thing to be aware of is that you are using `≈` with the default tolerances (about `1e-8` for `Float64`), so it will allow through matrices that are not orthogonal to the full accuracy. On the other hand, if you’re only using this for a sanity check then it should be reasonable.

> [@ufechner7](#):
>
> `R_ai = hcat(ax, az, ay)`

Note that this can be simplified to `R_ai = [ax az ay]`, which is essentially equivalent to an `hcat` call.

---

<div class="post-metadata">

**Author:** ![ufechner7](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/ufechner7/32/51363_2.png) [@ufechner7](https://discourse.julialang.org/u/ufechner7)\
**Post date:** [October 10, 2024, 5:33pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/27 "2024-10-10T17:33:55Z")

</div>

I am modifying my code a lot every day, and sometimes I make mistakes with the signs. I just want to catch this before getting stuck with follow-up errors. I just want to increase the trust in my results.

---

<div class="post-metadata">

**Author:** ![kellertuer](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/kellertuer/32/220707_2.png) [@kellertuer](https://discourse.julialang.org/u/kellertuer)\
**Post date:** [October 10, 2024, 6:15pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/28 "2024-10-10T18:15:31Z")

</div>

> [@stevengj](#):
>
> > [@kellertuer](#):
> >
> > I think replacing the `det` with the dot/cross one might be a bit more stable
> 
> I have no idea what this sentence means?

Not that important, mainly meant to do `is_right_handed(a,b,c) = dot(cross(a,b),c) > 0` instead since that might be more stable maybe?

---

<div class="post-metadata">

**Author:** ![stevengj](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/stevengj/32/71_2.png) [@stevengj](https://discourse.julialang.org/u/stevengj)\
**Post date:** [October 10, 2024, 7:18pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/29 "2024-10-10T19:18:43Z")

</div>

> [@kellertuer](#):
>
> `dot(cross(a,b),c) > 0` instead since that might be more stable maybe?

I’m not sure what you mean by “more stable”. `det([a b c])` essentially uses the same formula for a 3x3 `SMatrix`.

---

<div class="post-metadata">

**Author:** ![Ronis\_BR](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/ronis_br/32/50999_2.png) [@Ronis\_BR](https://discourse.julialang.org/u/Ronis_BR)\
**Post date:** [October 11, 2024, 12:09am UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/30 "2024-10-11T00:09:08Z")

</div>

Hi @ufechner7 !

Just for future information, here is our implementation of TRIAD:

```julia
"""
    triad(v_b::AbstractVector, v_a::AbstractVector, w_b::AbstractVector, w_a::AbstractVector; minimum_sine::Number) -> Bool, DCM

Compute the direction cosine matrix that rotates the coordinate system `a` into alignment
with the coordinate system `b` considering the measured vectors in `b` `v_b` and `w_b`, and
the reference vectors in `a` `v_a` and `w_a`.

The keyword `minimum_sine` contains the minimum value for the sine of the angle between the
vectors that allow the TRIAD computation. If those angles do not meet this criteria, nothing
will be computed.

# Returns

- `Bool`: `true` if TRIAD could be executed, `false` otherwise.
- `DCM`: DCM computed by TRIAD. If TRIAD could not be computed, the identity matrix is
    returned.
"""
function triad(
    v_b::AbstractVector{T1},
    v_a::AbstractVector{T2},
    w_b::AbstractVector{T3},
    w_a::AbstractVector{T4};
    minimum_sine::Number = 0.5
) where {T1<:Number, T2<:Number, T3<:Number, T4<:Number}
    T = promote_type(T1, T2, T3, T4)

    # Make sure `minimum_sine` is positive and larger than 0.001 (5°).
    sine_threshold = max(T(minimum_sine), T(0.001))

    # == Auxiliary Variables ===============================================================

    v_a_norm = norm(v_a)
    w_a_norm = norm(w_a)
    v_b_norm = norm(v_b)
    w_b_norm = norm(w_b)

    # To avoid division by zero, we must make sure that the norm is at least 1e-6.
    norm_threshold = 1e-6
    if ((v_a_norm < norm_threshold) ||
        (w_a_norm < norm_threshold) ||
        (v_b_norm < norm_threshold) ||
        (w_b_norm < norm_threshold))
        return false, DCM{T}(I)
    end

    v_x_w_a = v_a × w_a
    v_x_w_b = v_b × w_b

    v_x_w_a_norm = norm(v_x_w_a)
    v_x_w_b_norm = norm(v_x_w_b)

    # == Input Verification ================================================================

    # We must check the angle between the vectors in `a` and `b` reference systems because
    # of the noise.
    sin_wb_b = v_x_w_b_norm / (v_b_norm * w_b_norm)
    sin_wb_b < sine_threshold && return false, DCM{T}(I)

    sin_wb_a = v_x_w_a_norm / (v_a_norm * w_a_norm)
    sin_wb_a < sine_threshold && return false, DCM{T}(I)

    # == TRIAD =============================================================================

    t₁_b = v_b / v_b_norm
    t₁_a = v_a / v_a_norm

    t₂_b = v_x_w_b / v_x_w_b_norm
    t₂_a = v_x_w_a / v_x_w_a_norm

    t₃_b = t₁_b × t₂_b
    t₃_a = t₁_a × t₂_a

    M_b = DCM(
        t₁_b[1], t₂_b[1], t₃_b[1],
        t₁_b[2], t₂_b[2], t₃_b[2],
        t₁_b[3], t₂_b[3], t₃_b[3]
    )'

    M_a = DCM(
        t₁_a[1], t₂_a[1], t₃_a[1],
        t₁_a[2], t₂_a[2], t₃_a[2],
        t₁_a[3], t₂_a[3], t₃_a[3]
    )'

    return true, M_b * M_a'
end

```

---

<div class="post-metadata">

**Author:** ![ufechner7](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/ufechner7/32/51363_2.png) [@ufechner7](https://discourse.julialang.org/u/ufechner7)\
**Post date:** [October 11, 2024, 3:43am UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/31 "2024-10-11T03:43:34Z")

</div>

Thanks for sharing!

---

<div class="post-metadata">

**Author:** ![kellertuer](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/kellertuer/32/220707_2.png) [@kellertuer](https://discourse.julialang.org/u/kellertuer)\
**Post date:** [October 11, 2024, 5:41am UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/32 "2024-10-11T05:41:14Z")

</div>

Yes,right, then it was just too late to doo math yesterday for me.

---

<div class="post-metadata">

**Author:** ![moble](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/moble/32/23535_2.png) [@moble](https://discourse.julialang.org/u/moble)\
**Post date:** [October 11, 2024, 3:02pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/33 "2024-10-11T15:02:25Z")

</div>

> [@ufechner7](#):
>
> `http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm`

Interesting. I hadn’t run across that before. FYI, I’ve implemented another algorithm in my quaternion package [here](https://moble.github.io/Quaternionic.jl/dev/manual/#Alignment) that has always worked well for me — apparently called Davenport’s method. I guess TRIAD looks like it would be a bit faster, though that’s not usually my concern. But I would also guess that Davenport’s method is probably more robust.

[Previous page](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151.md?page=1)