Skip to content

Commit 9a57d43

Browse files
jbangelojbangelo
committed
Change reference_frame module to use more nalgebra
1 parent d914ea4 commit 9a57d43

File tree

2 files changed

+244
-560
lines changed

2 files changed

+244
-560
lines changed

‎swiftnav/src/reference_frame/mod.rs‎

Lines changed: 106 additions & 152 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,7 @@
7575
//!
7676
7777
use crate::coords::{Coordinate, ECEF};
78+
use nalgebra::{Matrix3, Vector3};
7879
use std::{
7980
collections::{HashMap, HashSet, VecDeque},
8081
fmt,
@@ -168,20 +169,12 @@ pub enum ReferenceFrame {
168169
/// formulation of the Helmert transformation.
169170
#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)]
170171
pub struct TimeDependentHelmertParams {
171-
tx: f64,
172-
tx_dot: f64,
173-
ty: f64,
174-
ty_dot: f64,
175-
tz: f64,
176-
tz_dot: f64,
172+
t: Vector3<f64>,
173+
t_dot: Vector3<f64>,
177174
s: f64,
178175
s_dot: f64,
179-
rx: f64,
180-
rx_dot: f64,
181-
ry: f64,
182-
ry_dot: f64,
183-
rz: f64,
184-
rz_dot: f64,
176+
r: Vector3<f64>,
177+
r_dot: Vector3<f64>,
185178
epoch: f64,
186179
}
187180

@@ -192,55 +185,40 @@ impl TimeDependentHelmertParams {
192185

193186
/// Reverses the transformation. Since this is a linear transformation we simply negate all terms
194187
pub fn invert(&mut self) {
195-
self.tx *= -1.0;
196-
self.tx_dot *= -1.0;
197-
self.ty *= -1.0;
198-
self.ty_dot *= -1.0;
199-
self.tz *= -1.0;
200-
self.tz_dot *= -1.0;
188+
self.t *= -1.0;
189+
self.t_dot *= -1.0;
201190
self.s *= -1.0;
202191
self.s_dot *= -1.0;
203-
self.rx *= -1.0;
204-
self.rx_dot *= -1.0;
205-
self.ry *= -1.0;
206-
self.ry_dot *= -1.0;
207-
self.rz *= -1.0;
208-
self.rz_dot *= -1.0;
192+
self.r *= -1.0;
193+
self.r_dot *= -1.0;
209194
}
210195

211196
/// Apply the transformation on a position at a specific epoch
212197
pub fn transform_position(&self, position: &ECEF, epoch: f64) -> ECEF {
213198
let dt = epoch - self.epoch;
214-
let tx = (self.tx + self.tx_dot * dt) * Self::TRANSLATE_SCALE;
215-
let ty = (self.ty + self.ty_dot * dt) * Self::TRANSLATE_SCALE;
216-
let tz = (self.tz + self.tz_dot * dt) * Self::TRANSLATE_SCALE;
199+
let t = (self.t + self.t_dot * dt) * Self::TRANSLATE_SCALE;
217200
let s = (self.s + self.s_dot * dt) * Self::SCALE_SCALE;
218-
let rx = (self.rx + self.rx_dot * dt) * Self::ROTATE_SCALE;
219-
let ry = (self.ry + self.ry_dot * dt) * Self::ROTATE_SCALE;
220-
let rz = (self.rz + self.rz_dot * dt) * Self::ROTATE_SCALE;
201+
let r = (self.r + self.r_dot * dt) * Self::ROTATE_SCALE;
221202

222-
let x = position.x() + tx + (s * position.x()) + (-rz * position.y()) + (ry * position.z());
223-
let y = position.y() + ty + (rz * position.x()) + (s * position.y()) + (-rx * position.z());
224-
let z = position.z() + tz + (-ry * position.x()) + (rx * position.y()) + (s * position.z());
203+
let m = Self::make_rotation_matrix(s, r);
225204

226-
ECEF::new(x, y, z)
205+
(position.as_vector() + t + m * position.as_vector()).into()
227206
}
228207

229208
/// Apply the transformation on a velocity at a specific position
230209
pub fn transform_velocity(&self, velocity: &ECEF, position: &ECEF) -> ECEF {
231-
let tx = self.tx_dot * Self::TRANSLATE_SCALE;
232-
let ty = self.ty_dot * Self::TRANSLATE_SCALE;
233-
let tz = self.tz_dot * Self::TRANSLATE_SCALE;
210+
let t = self.t_dot * Self::TRANSLATE_SCALE;
234211
let s = self.s_dot * Self::SCALE_SCALE;
235-
let rx = self.rx_dot * Self::ROTATE_SCALE;
236-
let ry = self.ry_dot * Self::ROTATE_SCALE;
237-
let rz = self.rz_dot * Self::ROTATE_SCALE;
212+
let r = self.r_dot * Self::ROTATE_SCALE;
238213

239-
let x = velocity.x() + tx + (s * position.x()) + (-rz * position.y()) + (ry * position.z());
240-
let y = velocity.y() + ty + (rz * position.x()) + (s * position.y()) + (-rx * position.z());
241-
let z = velocity.z() + tz + (-ry * position.x()) + (rx * position.y()) + (s * position.z());
214+
let m = Self::make_rotation_matrix(s, r);
242215

243-
ECEF::new(x, y, z)
216+
(velocity.as_vector() + t + m * position.as_vector()).into()
217+
}
218+
219+
#[must_use]
220+
fn make_rotation_matrix(s: f64, r: Vector3<f64>) -> Matrix3<f64> {
221+
Matrix3::new(s, -r.z, r.y, r.z, s, -r.x, -r.y, r.x, s)
244222
}
245223
}
246224

@@ -549,20 +527,20 @@ mod tests {
549527
#[test]
550528
fn helmert_position_translations() {
551529
let params = TimeDependentHelmertParams {
552-
tx: 1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
553-
tx_dot: 0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE,
554-
ty: 2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
555-
ty_dot: 0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE,
556-
tz: 3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
557-
tz_dot: 0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE,
530+
t: Vector3::new(
531+
1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
532+
2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
533+
3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
534+
),
535+
t_dot: Vector3::new(
536+
0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE,
537+
0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE,
538+
0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE,
539+
),
558540
s: 0.0,
559541
s_dot: 0.0,
560-
rx: 0.0,
561-
rx_dot: 0.0,
562-
ry: 0.0,
563-
ry_dot: 0.0,
564-
rz: 0.0,
565-
rz_dot: 0.0,
542+
r: Vector3::new(0.0, 0.0, 0.0),
543+
r_dot: Vector3::new(0.0, 0.0, 0.0),
566544
epoch: 2010.0,
567545
};
568546
let initial_position = ECEF::default();
@@ -581,20 +559,12 @@ mod tests {
581559
#[test]
582560
fn helmert_position_scaling() {
583561
let params = TimeDependentHelmertParams {
584-
tx: 0.0,
585-
tx_dot: 0.0,
586-
ty: 0.0,
587-
ty_dot: 0.0,
588-
tz: 0.0,
589-
tz_dot: 0.0,
562+
t: Vector3::new(0.0, 0.0, 0.0),
563+
t_dot: Vector3::new(0.0, 0.0, 0.0),
590564
s: 1.0 / TimeDependentHelmertParams::SCALE_SCALE,
591565
s_dot: 0.1 / TimeDependentHelmertParams::SCALE_SCALE,
592-
rx: 90.0,
593-
rx_dot: 0.0,
594-
ry: 0.0,
595-
ry_dot: 0.0,
596-
rz: 0.0,
597-
rz_dot: 0.0,
566+
r: Vector3::new(90.0, 0.0, 0.0),
567+
r_dot: Vector3::new(0.0, 0.0, 0.0),
598568
epoch: 2010.0,
599569
};
600570
let initial_position = ECEF::new(1., 2., 3.);
@@ -613,20 +583,20 @@ mod tests {
613583
#[test]
614584
fn helmert_position_rotations() {
615585
let params = TimeDependentHelmertParams {
616-
tx: 0.0,
617-
tx_dot: 0.0,
618-
ty: 0.0,
619-
ty_dot: 0.0,
620-
tz: 0.0,
621-
tz_dot: 0.0,
586+
t: Vector3::new(0.0, 0.0, 0.0),
587+
t_dot: Vector3::new(0.0, 0.0, 0.0),
622588
s: 0.0,
623589
s_dot: 0.0,
624-
rx: 1.0 / TimeDependentHelmertParams::ROTATE_SCALE,
625-
rx_dot: 0.1 / TimeDependentHelmertParams::ROTATE_SCALE,
626-
ry: 2.0 / TimeDependentHelmertParams::ROTATE_SCALE,
627-
ry_dot: 0.2 / TimeDependentHelmertParams::ROTATE_SCALE,
628-
rz: 3.0 / TimeDependentHelmertParams::ROTATE_SCALE,
629-
rz_dot: 0.3 / TimeDependentHelmertParams::ROTATE_SCALE,
590+
r: Vector3::new(
591+
1.0 / TimeDependentHelmertParams::ROTATE_SCALE,
592+
2.0 / TimeDependentHelmertParams::ROTATE_SCALE,
593+
3.0 / TimeDependentHelmertParams::ROTATE_SCALE,
594+
),
595+
r_dot: Vector3::new(
596+
0.1 / TimeDependentHelmertParams::ROTATE_SCALE,
597+
0.2 / TimeDependentHelmertParams::ROTATE_SCALE,
598+
0.3 / TimeDependentHelmertParams::ROTATE_SCALE,
599+
),
630600
epoch: 2010.0,
631601
};
632602
let initial_position = ECEF::new(1.0, 1.0, 1.0);
@@ -645,20 +615,20 @@ mod tests {
645615
#[test]
646616
fn helmert_velocity_translations() {
647617
let params = TimeDependentHelmertParams {
648-
tx: 1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
649-
tx_dot: 0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE,
650-
ty: 2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
651-
ty_dot: 0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE,
652-
tz: 3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
653-
tz_dot: 0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE,
618+
t: Vector3::new(
619+
1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
620+
2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
621+
3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE,
622+
),
623+
t_dot: Vector3::new(
624+
0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE,
625+
0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE,
626+
0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE,
627+
),
654628
s: 0.0,
655629
s_dot: 0.0,
656-
rx: 0.0,
657-
rx_dot: 0.0,
658-
ry: 0.0,
659-
ry_dot: 0.0,
660-
rz: 0.0,
661-
rz_dot: 0.0,
630+
r: Vector3::new(0.0, 0.0, 0.0),
631+
r_dot: Vector3::new(0.0, 0.0, 0.0),
662632
epoch: 2010.0,
663633
};
664634
let initial_velocity = ECEF::default();
@@ -673,20 +643,12 @@ mod tests {
673643
#[test]
674644
fn helmert_velocity_scaling() {
675645
let params = TimeDependentHelmertParams {
676-
tx: 0.0,
677-
tx_dot: 0.0,
678-
ty: 0.0,
679-
ty_dot: 0.0,
680-
tz: 0.0,
681-
tz_dot: 0.0,
646+
t: Vector3::new(0.0, 0.0, 0.0),
647+
t_dot: Vector3::new(0.0, 0.0, 0.0),
682648
s: 1.0 / TimeDependentHelmertParams::SCALE_SCALE,
683649
s_dot: 0.1 / TimeDependentHelmertParams::SCALE_SCALE,
684-
rx: 90.0,
685-
rx_dot: 0.0,
686-
ry: 0.0,
687-
ry_dot: 0.0,
688-
rz: 0.0,
689-
rz_dot: 0.0,
650+
r: Vector3::new(90.0, 0.0, 0.0),
651+
r_dot: Vector3::new(0.0, 0.0, 0.0),
690652
epoch: 2010.0,
691653
};
692654
let initial_velocity = ECEF::default();
@@ -701,20 +663,20 @@ mod tests {
701663
#[test]
702664
fn helmert_velocity_rotations() {
703665
let params = TimeDependentHelmertParams {
704-
tx: 0.0,
705-
tx_dot: 0.0,
706-
ty: 0.0,
707-
ty_dot: 0.0,
708-
tz: 0.0,
709-
tz_dot: 0.0,
666+
t: Vector3::new(0.0, 0.0, 0.0),
667+
t_dot: Vector3::new(0.0, 0.0, 0.0),
710668
s: 0.0,
711669
s_dot: 0.0,
712-
rx: 1.0 / TimeDependentHelmertParams::ROTATE_SCALE,
713-
rx_dot: 0.1 / TimeDependentHelmertParams::ROTATE_SCALE,
714-
ry: 2.0 / TimeDependentHelmertParams::ROTATE_SCALE,
715-
ry_dot: 0.2 / TimeDependentHelmertParams::ROTATE_SCALE,
716-
rz: 3.0 / TimeDependentHelmertParams::ROTATE_SCALE,
717-
rz_dot: 0.3 / TimeDependentHelmertParams::ROTATE_SCALE,
670+
r: Vector3::new(
671+
1.0 / TimeDependentHelmertParams::ROTATE_SCALE,
672+
2.0 / TimeDependentHelmertParams::ROTATE_SCALE,
673+
3.0 / TimeDependentHelmertParams::ROTATE_SCALE,
674+
),
675+
r_dot: Vector3::new(
676+
0.1 / TimeDependentHelmertParams::ROTATE_SCALE,
677+
0.2 / TimeDependentHelmertParams::ROTATE_SCALE,
678+
0.3 / TimeDependentHelmertParams::ROTATE_SCALE,
679+
),
718680
epoch: 2010.0,
719681
};
720682
let initial_velocity = ECEF::default();
@@ -729,53 +691,45 @@ mod tests {
729691
#[test]
730692
fn helmert_invert() {
731693
let mut params = TimeDependentHelmertParams {
732-
tx: 1.0,
733-
tx_dot: 0.1,
734-
ty: 2.0,
735-
ty_dot: 0.2,
736-
tz: 3.0,
737-
tz_dot: 0.3,
694+
t: Vector3::new(1.0, 2.0, 3.0),
695+
t_dot: Vector3::new(0.1, 0.2, 0.3),
738696
s: 4.0,
739697
s_dot: 0.4,
740-
rx: 5.0,
741-
rx_dot: 0.5,
742-
ry: 6.0,
743-
ry_dot: 0.6,
744-
rz: 7.0,
745-
rz_dot: 0.7,
698+
r: Vector3::new(5.0, 6.0, 7.0),
699+
r_dot: Vector3::new(0.5, 0.6, 0.7),
746700
epoch: 2010.0,
747701
};
748702
params.invert();
749-
assert_float_eq!(params.tx, -1.0, abs_all <= 1e-4);
750-
assert_float_eq!(params.tx_dot, -0.1, abs_all <= 1e-4);
751-
assert_float_eq!(params.ty, -2.0, abs_all <= 1e-4);
752-
assert_float_eq!(params.ty_dot, -0.2, abs_all <= 1e-4);
753-
assert_float_eq!(params.tz, -3.0, abs_all <= 1e-4);
754-
assert_float_eq!(params.tz_dot, -0.3, abs_all <= 1e-4);
703+
assert_float_eq!(params.t.x, -1.0, abs_all <= 1e-4);
704+
assert_float_eq!(params.t_dot.x, -0.1, abs_all <= 1e-4);
705+
assert_float_eq!(params.t.y, -2.0, abs_all <= 1e-4);
706+
assert_float_eq!(params.t_dot.y, -0.2, abs_all <= 1e-4);
707+
assert_float_eq!(params.t.z, -3.0, abs_all <= 1e-4);
708+
assert_float_eq!(params.t_dot.z, -0.3, abs_all <= 1e-4);
755709
assert_float_eq!(params.s, -4.0, abs_all <= 1e-4);
756710
assert_float_eq!(params.s_dot, -0.4, abs_all <= 1e-4);
757-
assert_float_eq!(params.rx, -5.0, abs_all <= 1e-4);
758-
assert_float_eq!(params.rx_dot, -0.5, abs_all <= 1e-4);
759-
assert_float_eq!(params.ry, -6.0, abs_all <= 1e-4);
760-
assert_float_eq!(params.ry_dot, -0.6, abs_all <= 1e-4);
761-
assert_float_eq!(params.rz, -7.0, abs_all <= 1e-4);
762-
assert_float_eq!(params.rz_dot, -0.7, abs_all <= 1e-4);
711+
assert_float_eq!(params.r.x, -5.0, abs_all <= 1e-4);
712+
assert_float_eq!(params.r_dot.x, -0.5, abs_all <= 1e-4);
713+
assert_float_eq!(params.r.y, -6.0, abs_all <= 1e-4);
714+
assert_float_eq!(params.r_dot.y, -0.6, abs_all <= 1e-4);
715+
assert_float_eq!(params.r.z, -7.0, abs_all <= 1e-4);
716+
assert_float_eq!(params.r_dot.z, -0.7, abs_all <= 1e-4);
763717
assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4);
764718
params.invert();
765-
assert_float_eq!(params.tx, 1.0, abs_all <= 1e-4);
766-
assert_float_eq!(params.tx_dot, 0.1, abs_all <= 1e-4);
767-
assert_float_eq!(params.ty, 2.0, abs_all <= 1e-4);
768-
assert_float_eq!(params.ty_dot, 0.2, abs_all <= 1e-4);
769-
assert_float_eq!(params.tz, 3.0, abs_all <= 1e-4);
770-
assert_float_eq!(params.tz_dot, 0.3, abs_all <= 1e-4);
719+
assert_float_eq!(params.t.x, 1.0, abs_all <= 1e-4);
720+
assert_float_eq!(params.t_dot.x, 0.1, abs_all <= 1e-4);
721+
assert_float_eq!(params.t.y, 2.0, abs_all <= 1e-4);
722+
assert_float_eq!(params.t_dot.y, 0.2, abs_all <= 1e-4);
723+
assert_float_eq!(params.t.z, 3.0, abs_all <= 1e-4);
724+
assert_float_eq!(params.t_dot.z, 0.3, abs_all <= 1e-4);
771725
assert_float_eq!(params.s, 4.0, abs_all <= 1e-4);
772726
assert_float_eq!(params.s_dot, 0.4, abs_all <= 1e-4);
773-
assert_float_eq!(params.rx, 5.0, abs_all <= 1e-4);
774-
assert_float_eq!(params.rx_dot, 0.5, abs_all <= 1e-4);
775-
assert_float_eq!(params.ry, 6.0, abs_all <= 1e-4);
776-
assert_float_eq!(params.ry_dot, 0.6, abs_all <= 1e-4);
777-
assert_float_eq!(params.rz, 7.0, abs_all <= 1e-4);
778-
assert_float_eq!(params.rz_dot, 0.7, abs_all <= 1e-4);
727+
assert_float_eq!(params.r.x, 5.0, abs_all <= 1e-4);
728+
assert_float_eq!(params.r_dot.x, 0.5, abs_all <= 1e-4);
729+
assert_float_eq!(params.r.y, 6.0, abs_all <= 1e-4);
730+
assert_float_eq!(params.r_dot.y, 0.6, abs_all <= 1e-4);
731+
assert_float_eq!(params.r.z, 7.0, abs_all <= 1e-4);
732+
assert_float_eq!(params.r_dot.z, 0.7, abs_all <= 1e-4);
779733
assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4);
780734
}
781735

0 commit comments

Comments
 (0)