# 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:** 20\
**Page:** 1

<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, 2:20pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/1 "2024-10-10T14:20:16Z")

</div>

I try to calculate the rotation matrix that needs to applied on one reference frame to match a second reference frame. I have the following code:

```julia
# test calculation of the orientation
using LinearAlgebra, Rotations, StaticArrays, Test
"""
    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)
    R_ai = hcat(ax, az, ay)
    R_bi = hcat(bx, bz, by)
    return R_bi * R_ai'
end

ax = [1, 0, 0] 
ay = [0, 1, 0] 
az = [0, 0, 1] 

x = [1, 0, 0] 
y = [0, 1, 0] 
z = [0, 0, 1]

rot1 = rot3d(ax, ay, az, x, y, z)
q1 = QuatRotation(rot1)
@test all(Rotations.params(q) .== SVector{4, Float64}([1.0 0 0 0]))

x = [-1, 0, 0] 
y = [0, 1, 0] 
z = [0, 0, 1]
rot2 = rot3d(ax, ay, az, x, y, z)
q2 = QuatRotation(rot2)

```

If I look at the result:

```julia
julia> q1
3×3 QuatRotation{Float64} with indices SOneTo(3)×SOneTo(3)(QuaternionF64(1.0, 0.0, 0.0, 0.0)):
 1.0 0.0 0.0
 0.0 1.0 0.0
 0.0 0.0 1.0

julia> q2
3×3 QuatRotation{Float64} with indices SOneTo(3)×SOneTo(3)(QuaternionF64(1.0, 0.0, 0.0, 0.0)):
 1.0 0.0 0.0
 0.0 1.0 0.0
 0.0 0.0 1.0

```

both rotations are the same, which cannot be correct. So there must be a mistake in my function `rot3d`.

How can I implement it correctly?

---

<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, 2:50pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/2 "2024-10-10T14:50:48Z")

</div>

I think the issue is because your new frame is left handed instead of right handed, so there is no pure rotation from the right handed lab frame to `[x y z]`

The transformation you calculate is correct:

```julia
@assert [ax ay az] ≈ rot2 * [x y z]

```

I’ve never used the rotations package, so I don’t know what `QuatRotation` does when it’s given a left handed frame.

---

<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, 2:56pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/3 "2024-10-10T14:56:35Z")

</div>

How can I write a function that checks if a rotation frame is right handed?

---

<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, 2:59pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/4 "2024-10-10T14:59:58Z")

</div>

You can check

```julia
x = [-1,0,0]
y = [0,1,0]
z = [0,0,1]

cross(x,y) ≈ z
#or
det([x y z]) ≈ 1

```

---

<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, 3:56pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/5 "2024-10-10T15:56:25Z")

</div>

Ok, I defined now

```julia
function is_right_handed(x, y, z)
    return det([x y z]) ≈ 1
end

```

and use an assert to check the input vectors.

But what is this function actually doing? I think it actually checks something else, because:

```julia
ax = [2, 0, 0] 
ay = [0, 1, 0] 
az = [0, 0, 1]
@assert is_right_handed(ax, ay, az)

```

fails.

---

<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, 4:08pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/6 "2024-10-10T16:08:08Z")

</div>

At least in your own docs you wrote that the vectors all must already be normalised? your `ax` is not normalised.

Checking the determinant to be 1 (approx) for right, or -1 for left handed only works for unit frames (all vectors unit norm and orthogonal)

---

<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, 4:09pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/7 "2024-10-10T16:09:33Z")

</div>

So the function should be called: `is_right_handed_and_normalized()`?

But does it really check that?

I just need a function that checks the required pre-conditions so that I can make my code more robust.

---

<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, 4:16pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/8 "2024-10-10T16:16:43Z")

</div>

So wikipedia:

> **[Right-hand rule](https://en.wikipedia.org/wiki/Right-hand_rule#History)**
>
> In mathematics and physics, the right-hand rule is a convention and a mnemonic, utilized to define the orientation of axes in three-dimensional space and to determine the direction of the cross product of two vectors, as well as to establish the direction of the force on a current-carrying conductor in a magnetic field. 
> The various right- and left-hand rules arise from the fact that the three axes of three-dimensional space have two possible orientations. This can be seen by holding your hands ...

says: If the cross product points the same direction as your third direction is is right-handed, otherwise left-handed.  
If you have unit vectors, that is even just checking for ±1.  
The formula form the determinant comes from [Cross product - Wikipedia](https://en.wikipedia.org/wiki/Cross_product#Matrix_notation)

but you probably, for arbitrary vectors, you also just check the sign of \langle a \times b, c\rangle.  
At least for an orthogonal (but not necessarily normal) frame, this would give you a negative number if you have a right-handed system or a positive for left-handed (probably more stable than checking against a 1).

---

<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, 4:21pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/9 "2024-10-10T16:21:03Z")

</div>

Ah, do not ask a mathematician after 6pm to do all signs right. So if you do

```julia
using LinearAlgebra
is_right_handed(a,b,c) = dot(cross(a,b),c) > 0

```

That should just work fine?

For you example

```julia
julia> is_right_handed(ax,ay,az)
true

```

---

<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, 4:21pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/10 "2024-10-10T16:21:06Z")

</div>

Like this:

```julia
function is_right_handed_normalized(x, y, z)
    if !norm(x) ≈ 1 || !norm(y) ≈ 1 || !norm(z) ≈ 1
        return false
    end
    return det([x y z]) ≈ 1
end

```

?  
But does that already guarantee that the vectors are orthonormal?

---

<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, 4:22pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/11 "2024-10-10T16:22:31Z")

</div>

Ah, so you. do not only want to check whether it is right-handed but even more whether it is orthonormal?

For that you have to check all norms and all pairwise inner products (all of them have to be 1)

_edit:_ I read you post above in a way, that you already _have_ a rotation frame – that is some rotation matrix; then you would know the ONB property already. So what do you actually have as input then?

---

<div class="post-metadata">

**Author:** ![juliohm](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/juliohm/32/215266_2.png) [@juliohm](https://discourse.julialang.org/u/juliohm)\
**Post date:** [October 10, 2024, 4:29pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/12 "2024-10-10T16:29:53Z")

</div>

Notice that Rotations.jl already provides a utility function `rotation_between(u, v)` to create a rotation from vector `u` to `v`. So you could compose these rotations to match the three axes of the frames.

---

<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, 4:37pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/13 "2024-10-10T16:37:57Z")

</div>

> [@kellertuer](#):
>
> inner products

What are inner products? Cross products or dot products or something else?

---

<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, 4:39pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/14 "2024-10-10T16:39:50Z")

</div>

eh inner products are just dot products, sorry for not being clear (cf. [Inner product space - Wikipedia](https://en.wikipedia.org/wiki/Inner_product_space#Definition)).

---

<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, 4:42pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/15 "2024-10-10T16:42:02Z")

</div>

But that cannot be correct. You said the inner products must be one. But they are not:

```julia
julia> x
3-element Vector{Int64}:
 0
 1
 0

julia> y
3-element Vector{Int64}:
 1
 0
 0

julia> (x ⋅ y)
0

```

---

<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, 4:44pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/16 "2024-10-10T16:44:15Z")

</div>

Sorry it’s late so I should probably stop here. What I meant was: Inner products of vectors with themselves have to be 1, with others zero.

Or, if you write them column wise into a matrix you get X = [a\ b\ c] you can also write this as X^{\mathrm{T}}X=I, which is also nothing else than classifying a rotation matrix if then the determinant is +1 – otherwise there is a mirroring happening as well.

---

<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, 4:48pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/17 "2024-10-10T16:48:11Z")

</div>

Oh, and if you do not mind the mirroring I mentioned in the last task your original task can also be solved by an SVD [Orthogonal Procrustes problem - Wikipedia](https://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem)

---

<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, 4:50pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/18 "2024-10-10T16:50:46Z")

</div>

Thanks, now I have this code:

```julia
function is_right_handed_orthonormal(x, y, z)
    if !(norm(x) ≈ 1) || !(norm(y) ≈ 1) || !(norm(z) ≈ 1)
        return false
    end
    if !((x ⋅ y) ≈ 0) || !((y ⋅ z) ≈ 0) || !((z ⋅ x) ≈ 0)
        return false
    end
    return det([x y z]) ≈ 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 = hcat(ax, az, ay)
    R_bi = hcat(bx, bz, by)
    return R_bi * R_ai'
end

```

Does that look correct now?

---

<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, 4:52pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/19 "2024-10-10T16:52:21Z")

</div>

Yeah that looks good. I think replacing the `det` with the dot/cross one might be a bit more stable, but maybe there is also not much difference in practice.

---

<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, 4:53pm UTC](https://discourse.julialang.org/t/how-can-i-implement-the-triad-algorithm-in-julia/121151/20 "2024-10-10T16:53:50Z")

</div>

Thank you so much!

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