Skip to content
Snippets Groups Projects
Commit f5980d6c authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'devel' into 287-tris

parents c8907d60 8c691915
No related branches found
No related tags found
1 merge request!358WIP: Resolve "Complete state vector new data structure?"
Pipeline #5344 failed
...@@ -275,72 +275,79 @@ int main() ...@@ -275,72 +275,79 @@ int main()
* *
* P: wolf tree status --------------------- * P: wolf tree status ---------------------
Hardware Hardware
S1 ODOM 2d // Sensor 1, type ODOMETRY 2d. Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below) sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
pm1 ODOM 2d // Processor 1, type ODOMETRY 2d o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
o: C7 - F3 // origin at Capture 7, Frame 3 l: Cap8 - Frm4 // last at Capture 8, Frame 4
l: C10 - F4 // last at Capture 10, frame 4 Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
S2 RANGE BEARING // Sensor 2, type RANGE and BEARING. sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below) Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type rande and bearing
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
pt2 RANGE BEARING // Processor 2: type Range and Bearing
Trajectory Trajectory
KF1 <-- c3 // KeyFrame 1, constrained by Factor 3 KFrm1 <-- Fac4 //KeyFrame 1, constrained by Factor 4
Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector P[Est] = ( -4.4e-12 1.5e-09 ) //Position is estimated
sb: Est Est // State's pos and orient are estimated O[Est] = ( 2.6e-09 ) //Orientation is estimated
C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
f1 FIX <-- // Feature 1, type Fix Ftr1 trk0 prior <-- // Position prior
m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin m = ( 0 0 )
c1 FIX --> A // Factor 1, type FIX, it is Absolute Fac1 FactorBlockAbsolute --> Abs
CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr) Ftr2 trk0 prior <-- // Orientation prior
C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr) m = ( 0 )
f2 RANGE BEARING <-- // Feature 2, type R+B Fac2 FactorBlockAbsolute --> Abs
m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1 buffer size : 0
KF2 <-- c6 Cap4 CaptureRangeBearing -> Sen2 <--
Est, ts=1, x = ( 1 2.5e-10 1.6e-10 ) Ftr3 trk0 FeatureRangeBearing <--
sb: Est Est m = ( 1 1.6 )
CM3 ODOM 2d -> S1 [Sta, Sta] <-- Fac3 RANGE BEARING --> Lmk1
f3 ODOM 2d <-- KFrm2 <-- Fac7
P[Est] = ( 1 5.7e-09 )
O[Est] = ( 5.7e-09 )
CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <-- // Capture 3 of type motion odom2d from sensor 1.
// Its origin is at Capture 2; Frame 1
buffer size : 2
delta preint : (1 0 0)
Ftr4 trk0 ProcessorOdom2d <--
m = ( 1 0 0 ) m = ( 1 0 0 )
c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1 Fac4 FactorOdom2d --> Frm1
C9 RANGE BEARING -> S2 [Sta, Sta] <-- Cap7 CaptureRangeBearing -> Sen2 <--
f4 RANGE BEARING <-- Ftr5 trk0 FeatureRangeBearing <--
m = ( 1.41 2.36 ) m = ( 1.4 2.4 )
c4 RANGE BEARING --> L1 Fac5 RANGE BEARING --> Lmk1
f5 RANGE BEARING <-- Ftr6 trk0 FeatureRangeBearing <--
m = ( 1 1.57 ) m = ( 1 1.6 )
c5 RANGE BEARING --> L2 Fac6 RANGE BEARING --> Lmk2
KF3 <-- KFrm3 <--
Est, ts=2, x = ( 2 4.1e-10 1.7e-10 ) P[Est] = ( 2 1.2e-08 )
sb: Est Est O[Est] = ( 6.6e-09 )
CM7 ODOM 2d -> S1 [Sta, Sta] <-- CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
f6 ODOM 2d <-- buffer size : 2
delta preint : (1 0 0)
Ftr7 trk0 ProcessorOdom2d <--
m = ( 1 0 0 ) m = ( 1 0 0 )
c6 ODOM 2d --> F2 Fac7 FactorOdom2d --> Frm2
C12 RANGE BEARING -> S2 [Sta, Sta] <-- Cap9 CaptureRangeBearing -> Sen2 <--
f7 RANGE BEARING <-- Ftr8 trk0 FeatureRangeBearing <--
m = ( 1.41 2.36 ) m = ( 1.4 2.4 )
c7 RANGE BEARING --> L2 Fac8 RANGE BEARING --> Lmk2
f8 RANGE BEARING <-- Ftr9 trk0 FeatureRangeBearing <--
m = ( 1 1.57 ) m = ( 1 1.6 )
c8 RANGE BEARING --> L3 Fac9 RANGE BEARING --> Lmk3
F4 <-- Frm4 <--
Est, ts=2, x = ( 0.11 -0.045 0.26 ) P[Est] = ( 2 0 )
sb: Est Est O[Est] = ( 0 )
CM10 ODOM 2d -> S1 [Sta, Sta] <-- CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
buffer size : 1
delta preint : (0 0 0)
Map Map
L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4 Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by facotrs 3 & 5
Est, x = ( 1 2 ) // L4 state is estimated, state vector P[Est] = ( 1 2 )
sb: Est // L4 has 1 state block estimated Lmk2 LandmarkPoint2d <-- Fac6 Fac8
L2 POINT 2d <-- c5 c7 P[Est] = ( 2 2 )
Est, x = ( 2 2 ) Lmk3 LandmarkPoint2d <-- Fac9
sb: Est P[Est] = ( 3 2 )
L3 POINT 2d <-- c8
Est, x = ( 3 2 )
sb: Est
----------------------------------------- -----------------------------------------
* *
* ============= GENERAL WOLF NOTES ================== * ============= GENERAL WOLF NOTES ==================
* *
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment