Inertia navigation and landmarks tracking #907

A-acuto · 2023-12-12T17:14:17Z

In this PR we present a set of measurement models regarding inertia navigation, in particular the measurement models from accelerometer and gyroscope on board of sensors. To use these measurement models a set of navigation functions have been added (including tests).
Summarising this PR consists in:

Navigation functions and Euler angles calculations;
Accelerometer measurement model
Gyroscope Measurement model
Azimuth-Elevation and Azimuth-Elevation-Range measurement model with fixed targets (for navigation calibration)
an example of tracking using these measurement models.
set of tests for the functions and measurement models.

…asing how does it work. Added tests and checks to the functions, measurements models.

…en UKF, EKF and Particle filters. Updated the measurement model to work with EKF and PF. Fixed a string that was causing the test to fail.

…cuto/Stone-Soup into inertia_navigation_landmarks

A-acuto · 2023-12-14T17:34:25Z

In the comparison example I was not able to use the metric manager (in particular the SIAP metric) because the track associator was trying to iterate the GroundTruthState (doing something like next(state) from the GroundTruthPath) and it was failing. Therefore I used a less elegant solution to show the positional accuracy.

Here is a gist that shows the problem:
https://gist.github.com/A-acuto/3c182d049e4df181e35c2a82307544fc

sdhiscocks

Had an initial look at the code. Main points: being consistent with radians, rather than degrees; and to double check if a different angle convention is being used; a few places where functions could be vectorised (included some suggestions but may not be best approach).

stonesoup/functions/__init__.py

stonesoup/functions/navigation.py

stonesoup/models/measurement/nonlinear.py

A-acuto · 2024-01-11T09:28:40Z

@sdhiscocks thanks for the comments and suggestions, I'll apply them and double check the degree-radians transformations to keep the code uniform. As well, I'll check if PyMap3D does contain functions needed.

…niformed the angles to radians, removed functions already present in the code, grammar checks and documentation

A-acuto · 2024-01-31T12:07:27Z

In the last commits I answered the comments provided, in details:

removed functions already present (e.g. angle_wrap -> mod_bearing)
made extensive use of PyMap3D for distances and functions
vectorisation of functions
switch from degrees to radians and alignment with stone soup rotation order
grammar improvements and checks in the examples

The only point I am not sure about, but as presented works fine, is the use of function Build_rotation_matrix which assumes angles being 1-dimensional, which should be fine when dealing with Kalman filter tracking but not for Particle filter. However allowing it to be multi-dimensional (arrays of (3, N) dimension) is messing up the tracking with the particle filter.

csherman-dstl

I've left some comments generally on the wording in the examples and some PEP styling on function names. All functions and function variables in init.py and navigation.py should be named in snake_case.

csherman-dstl · 2024-03-11T09:37:14Z

docs/examples/comparison_filters_on_navigation.py

+    -----------
+    target_speed: float
+        Speed of the sensor;
+    target_tadius: float


Suggested change

target_tadius: float

target_radius: float

csherman-dstl · 2024-03-11T09:39:13Z

docs/examples/comparison_filters_on_navigation.py

+    start_time: datetime,
+        start of the simulation;
+    number_of_timesteps: np.array
+        simulation lenght


Suggested change

simulation lenght

simulation length

csherman-dstl · 2024-03-11T10:14:02Z

docs/examples/example_navigation.py

+# accelerometer and gyroscope, with fixed target locations, also refereed as landmarks.
+# In this example, we simulate a three dimensional sensor, moving in 3D cartesian space,
+# we have the measurements from on-board instruments that evaluates the Euler angles, whose describe the
+# sensor rotations and orientation during the flight, as well as the the 3D forces acting on the sensor.


Suggested change

# sensor rotations and orientation during the flight, as well as the the 3D forces acting on the sensor.

# sensor rotations and orientation during the flight, as well as the 3D forces acting on the sensor.

csherman-dstl · 2024-03-11T10:17:06Z

docs/examples/example_navigation.py

+# In this example, we present how to perform the tracking task using an inertia
+# navigation measurement model making use of instruments mounted on the sensor.
+# This example is relevant for tracking sensors in environments where GPS tracking is not
+# available and we integrate the information obtained from instruments on board, as the


Suggested change

# available and we integrate the information obtained from instruments on board, as the

# available and we integrate the information obtained from instruments on board, the

csherman-dstl · 2024-03-11T10:26:12Z

docs/examples/example_navigation.py

+# model for the z-coordinate, since the sensor is moving on a fixed plane at 1 km above the surface.
+# At this stage we can start collecting both the groundtruths and # the measurement using a composite
+# measurement model merging the measurements from the :class:`~.AccelerometerMeasurementModel`,
+# the :class:`~.GyroscopeMeasurementModel` and the landmarks, using an


Suggested change

# the :class:`~.GyroscopeMeasurementModel` and the landmarks, using an

# the :class:`~.GyroscopeMeasurementModel` and the landmarks, using a

csherman-dstl · 2024-03-11T10:27:31Z

docs/examples/example_navigation.py

+# the :class:`~.GyroscopeMeasurementModel` and the landmarks, using an
+# :class:`~.CartesianAzimuthElevationMeasurementModel`.
+# This measurement model combines the specific forces measured by the accelerometer instrument
+# and the angular rotation from the inertia movements of the target. The landmarks helps reducing the


Suggested change

# and the angular rotation from the inertia movements of the target. The landmarks helps reducing the

# and the angular rotation from the inertia movements of the target. The landmarks help to reduce the

csherman-dstl · 2024-03-11T10:49:15Z

stonesoup/functions/__init__.py

+    return (latitude, longitude, altitude)
+
+
+def localSphere2GCS(xEast, yNorth, zUp, origin):


Suggested change

def localSphere2GCS(xEast, yNorth, zUp, origin):

def local_sphere2GCS(x_east, y_north, z_up, origin):

According to PEP-8 function and function variables should be in snake case.

csherman-dstl · 2024-03-11T10:51:28Z

stonesoup/functions/navigation.py

+from . import localSphere2GCS, build_rotation_matrix
+
+
+def earthSpeedFlatSq(dx, dy):


Suggested change

def earthSpeedFlatSq(dx, dy):

def earth_speed_flat_sq(dx, dy):

Snake case should be used for function names. This is the case for all functions in this file.

csherman-dstl · 2024-03-11T11:08:39Z

stonesoup/functions/navigation.py

+    """
+
+    # mapping
+    position_mapping = (0, 3, 6)


Should these mappings be arguments for generality?

…ngs for functions, fixed visualisation of examples

Added various navigation like measurement models and an example showc…

3a996d6

…asing how does it work. Added tests and checks to the functions, measurements models.

A-acuto requested a review from a team as a code owner December 12, 2023 17:14

A-acuto requested review from sdhiscocks and csherman-dstl and removed request for a team December 12, 2023 17:14

jswright-dstl assigned jswright-dstl and hpritchett-dstl Dec 12, 2023

jswright-dstl requested review from jswright-dstl and hpritchett-dstl December 12, 2023 19:24

jswright-dstl unassigned jswright-dstl and hpritchett-dstl Dec 12, 2023

jswright-dstl and others added 5 commits December 13, 2023 17:30

Fix Flake8 errors

1b0b3db

Remove unused variables and correct function misspellings

f84b5fd

Adding a comparison example using navigation measurement models betwe…

037af05

…en UKF, EKF and Particle filters. Updated the measurement model to work with EKF and PF. Fixed a string that was causing the test to fail.

Adding a comparison example using navigation measurement models betwe…

f09fc65

…en UKF, EKF and Particle filters. Updated the measurement model to work with EKF and PF. Fixed a string that was causing the test to fail.

Merge branch 'inertia_navigation_landmarks' of https://github.com/A-a…

235331e

…cuto/Stone-Soup into inertia_navigation_landmarks

sdhiscocks reviewed Jan 10, 2024

View reviewed changes

Upgrades in the codes, vectorised functions and measurement models, u…

a08ae07

…niformed the angles to radians, removed functions already present in the code, grammar checks and documentation

A-acuto added 2 commits January 31, 2024 14:56

Flake8 and test fixes, fix on the text of the examples

ce6e025

missing flake8 fix

21d2409

csherman-dstl reviewed Mar 11, 2024

View reviewed changes

A-acuto added 4 commits March 20, 2024 11:42

functions renaming to match PEP-8 requirements, generalised the mappi…

07b8298

…ngs for functions, fixed visualisation of examples

flake8, visualisation and text of the examples fixes

0bc0938

change from StateVector to Statevectors in measurement model

7cb20f9

flake 8 fix

c96bcab

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Inertia navigation and landmarks tracking #907

Inertia navigation and landmarks tracking #907

A-acuto commented Dec 12, 2023

A-acuto commented Dec 14, 2023 •

edited

sdhiscocks left a comment

A-acuto commented Jan 11, 2024

A-acuto commented Jan 31, 2024

csherman-dstl left a comment

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

csherman-dstl Mar 11, 2024

	# sensor rotations and orientation during the flight, as well as the the 3D forces acting on the sensor.
	# sensor rotations and orientation during the flight, as well as the 3D forces acting on the sensor.

	# available and we integrate the information obtained from instruments on board, as the
	# available and we integrate the information obtained from instruments on board, the

	# the :class:`~.GyroscopeMeasurementModel` and the landmarks, using an
	# the :class:`~.GyroscopeMeasurementModel` and the landmarks, using a

	# and the angular rotation from the inertia movements of the target. The landmarks helps reducing the
	# and the angular rotation from the inertia movements of the target. The landmarks help to reduce the

		return (latitude, longitude, altitude)


		def localSphere2GCS(xEast, yNorth, zUp, origin):

	def localSphere2GCS(xEast, yNorth, zUp, origin):
	def local_sphere2GCS(x_east, y_north, z_up, origin):

		from . import localSphere2GCS, build_rotation_matrix


		def earthSpeedFlatSq(dx, dy):

	def earthSpeedFlatSq(dx, dy):
	def earth_speed_flat_sq(dx, dy):

Inertia navigation and landmarks tracking #907

Are you sure you want to change the base?

Inertia navigation and landmarks tracking #907

Conversation

A-acuto commented Dec 12, 2023

A-acuto commented Dec 14, 2023 • edited

sdhiscocks left a comment

Choose a reason for hiding this comment

A-acuto commented Jan 11, 2024

A-acuto commented Jan 31, 2024

csherman-dstl left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

A-acuto commented Dec 14, 2023 •

edited