diff --git a/docs/examples/comparison_filters_on_navigation.py b/docs/examples/comparison_filters_on_navigation.py new file mode 100644 index 000000000..59eff3e81 --- /dev/null +++ b/docs/examples/comparison_filters_on_navigation.py @@ -0,0 +1,412 @@ +#!/usr/bin/env python +# coding: utf-8 + +""" +==================================================================== +Comparing different tracking algorithm using navigation measurements +==================================================================== +""" + +# %% +# This example compares the performances of various filters in tracking objects with +# navigation-like measurements models. We are interested in this scenario to show how we can use +# measurements models in the navigation context, and how different tracking algorithms perform. +# +# In an different example, we have explained how to set up the problem involving Euler angles and +# forces acting onto a sensor, using measurement models components coming from +# :class:`~.AccelerometerMeasurementModel`, :class:`~.GyroscopeMeasurementModel` and fixed targets, landmarks, +# in Stone soup. +# This example will show the performances in a 1-to-1 comparison using Extended Kalman filter (EKF), +# Unscented Kalman Filter (UKF) and Particle filter (PF) in a single target-sensor scenario. +# +# This example follows this schema: +# 1. Instantiate the target-sensor ground truths and gather the measurements; +# 2. Prepare and load the various filters components; +# 3. Run the trackers and obtain the tracks; +# 4. Create and visualise the performances of the tracking algorithms. +# + +# %% +# General imports +# ^^^^^^^^^^^^^^^ + +import numpy as np +from datetime import datetime, timedelta +from scipy.stats import multivariate_normal +import matplotlib.pyplot as plt + +# %% +# Stone Soup imports +# ^^^^^^^^^^^^^^^^^^ + +from stonesoup.models.transition.linear import CombinedGaussianTransitionModel, \ + ConstantAcceleration, ConstantVelocity, Singer, CombinedLinearGaussianTransitionModel +from stonesoup.types.groundtruth import GroundTruthState, GroundTruthPath +from stonesoup.types.state import GaussianState +from stonesoup.types.array import StateVector +from stonesoup.types.detection import Detection +from stonesoup.functions.navigation import get_eulers_angles + + +# Simulation parameters +np.random.seed(2010) # fix a random seed +simulation_steps = 100 +timesteps = np.linspace(1, simulation_steps+1, simulation_steps+1) +start_time = datetime.now().replace(microsecond=0) +# Lets assume a sensor with these specifics +radius = 5000 +speed = 200 +center = np.array([0, 0, 1000]) # latitude, longitude, altitutde (meters) + +# %% +# 1) Instantiate the target ground truth path +# ------------------------------------------- +# For this example we consider a different approach for describing the target ground truth. +# We evaluate the sensor motion on a circular trajectory by modelling simply the 3D movements +# and measure the Euler angles associated with the object direction. +# + +from stonesoup.types.detection import TrueDetection + +# Create a function to create the groundtruth paths +def describe_sensor_motion(target_speed: float, + target_radius: float, + starting_position: np.array, + start_time: datetime, + number_of_timesteps: np.array + ) -> (list, set): + + """ + Auxuliary function to create the sensor-target dynamics in the + specific case of circular motion. + + Parameters: + ----------- + target_speed: float + Speed of the sensor; + target_radius: float + radius of the circular trajectory; + starting_position: np.array + starting point of the trajectory, latitude, longitude + and altitude; + start_time: datetime, + start of the simulation; + number_of_timesteps: np.array + simulation lenght + + Return: + ------- + (list, set): + list of timestamps of the simulation and + groundtruths path. + """ + + # Instantiate the 15 dimension object describing + # the positions, dynamics and angles of the target + sensor_dynamics = np.zeros((15)) + + # Generate the groundTruthpath + truths = GroundTruthPath([]) + + # instantiate a list for the timestamps + timestamps = [] + + # indexes of the array + position_indexes = [0, 3, 6] + velocity_indexes = [1, 4, 7] + acceleration_indexes = [2, 5, 8] + angles_indexes = [9, 11, 13] + vangles_indexes = [10, 12, 14] + + # loop over the timestep + for i in number_of_timesteps: + theta = target_speed * i / target_radius + 0 + + # positions + sensor_dynamics[position_indexes] += target_radius * \ + np.array([np.cos(theta), np.sin(theta), + 0.001*np.random.choice(np.arange(-5, 5), 1)[0]]) + \ + starting_position + + # velocities + sensor_dynamics[velocity_indexes] += target_speed * \ + np.array([-np.sin(theta), np.cos(theta), 0]) + + # acceleration + sensor_dynamics[acceleration_indexes] += ((-target_speed * target_speed) / target_radius) * \ + np.array([np.cos(theta), np.sin(theta), 0]) + + # Now using the velocity and accelerations terms we get the Euler angles + angles, dangles = get_eulers_angles(sensor_dynamics[velocity_indexes], + sensor_dynamics[acceleration_indexes]) + + # add the Euler angles and their time derivative + # please check that are all angles + sensor_dynamics[angles_indexes] += angles + sensor_dynamics[vangles_indexes] += dangles + + # append all those as ground state + truths.append(GroundTruthState(state_vector=sensor_dynamics, + timestamp=start_time + + timedelta(seconds=int(i)))) + # restart the array + sensor_dynamics = np.zeros((15)) + timestamps.append(start_time + timedelta(seconds=int(i))) + + return (timestamps, truths) + + +# Instantiate the transition model, We consider the Singer model for +# an exponential declining acceleration in the z-coordinate. +transition_model = CombinedLinearGaussianTransitionModel([ + ConstantAcceleration(1.5), + ConstantAcceleration(1.5), + Singer(0.1, 10), + ConstantVelocity(0), + ConstantVelocity(0), + ConstantVelocity(0) + ]) + + +# Generate the ground truths +timestamps, groundtruths = describe_sensor_motion(speed, radius, center, start_time, + timesteps) +# %% +# Load the measurement model +# ^^^^^^^^^^^^^^^^^^^^^^^^^^ +# As for the other example, we compose our measurement model +# using the accelearation, gyroscope and landmarks measurement models. +# +# For the landmarks we consider a :class:`~.CartesianAzimuthElevationRangeMeasurementModel` +# which provides the Azimuth, Elevation and Range between the +# sensor and the fixed target to ease the tracking efficiency. +# + +from stonesoup.models.measurement.nonlinear import AccelerometerMeasurementModel, GyroscopeMeasurementModel, \ + CartesianAzimuthElevationRangeMeasurementModel, CombinedReversibleGaussianMeasurementModel + +# Instantiate the measurement model +measurement_model_list = [] + +# Instantiate the landmarks - the z-coordinate is randomly drawn +target1 = np.array([3000, 3000, 0.0096]) +target2 = np.array([-3000, 3000, 1.6034]) +target3 = np.array([0, -3000, 0.93]) +targets = [target1, target2, target3] + +# Instantantiate a reference frame for the Gravity +# forces +reference_frame = StateVector([55, 0, 0]) # Latitude, longitude, Altitude + +# Model list +measurement_model_list = [] + +accelerometer = AccelerometerMeasurementModel( + ndim_state=15, + mapping=(0, 3, 6), + noise_covar=np.diag([1, 1, 10]), # Acceleration + reference_frame=reference_frame +) + +gyroscope = GyroscopeMeasurementModel( + ndim_state=15, + mapping=(0, 3, 6), + noise_covar=np.diag([1e-5, 1e-5, 1e-5]), # Gyroscope, noise in micro radiands + reference_frame=reference_frame +) + +# add the measurements models +measurement_model_list.append(accelerometer) +measurement_model_list.append(gyroscope) + +# loop over the various targets to initilise the +# azimuth-elevation-range models. +for target in targets: + measurement_model_list.append( + CartesianAzimuthElevationRangeMeasurementModel( + ndim_state=15, + mapping=(0, 3, 6), + noise_covar=np.diag([1, 1, 10]), + target_location=StateVector(target), + translation_offset=None) + ) + + +measurement_model = CombinedReversibleGaussianMeasurementModel(measurement_model_list) +measurements_set = [] + +# Now create the measurements +for truth in groundtruths: + measurement = measurement_model.function(truth, noise=True) + measurements_set.append(Detection(state_vector=measurement, + timestamp=truth.timestamp, + measurement_model=measurement_model)) +# %% +# 2) Prepare and load the various filters components; +# --------------------------------------------------- +# So far we have generated the sensor original track and +# we have gathered the measurements using the :class:`~.CombinedReversibleGaussianMeasurementModel`. +# We can now load the various filter components for the EKF, UKF and PF. +# Then, we need to instantiate the components as the prior and the tracks. + +# Load the Kalman components +from stonesoup.updater.kalman import UnscentedKalmanUpdater, ExtendedKalmanUpdater +from stonesoup.predictor.kalman import UnscentedKalmanPredictor, ExtendedKalmanPredictor + +# Load the Particle filter components +from stonesoup.updater.particle import ParticleUpdater +from stonesoup.predictor.particle import ParticlePredictor +from stonesoup.resampler.particle import ESSResampler + +# Extended Kalman filter +EKF_predictor = ExtendedKalmanPredictor(transition_model) +EKF_updater = ExtendedKalmanUpdater(measurement_model=None) + +# Unscented Kalman filter +UKF_predictor = UnscentedKalmanPredictor(transition_model) +UKF_updater = UnscentedKalmanUpdater(measurement_model=None) + +# Particle filter +PF_predictor = ParticlePredictor(transition_model) +resampler = ESSResampler() +PF_updater = ParticleUpdater(measurement_model=None, + resampler=resampler) + +# Create a starting covarinace +covar_starting_position = np.repeat(10, 15) + +# Instantiate the prior, with a known location. +prior = GaussianState( + state_vector=groundtruths[0].state_vector, + covar=np.diag(covar_starting_position), + timestamp=timestamps[0]) + +# instantate the PF prior +from stonesoup.types.state import ParticleState +from stonesoup.types.numeric import Probability +from stonesoup.types.particle import Particle + +# Number of particles +number_particles=1024 + +samples = multivariate_normal.rvs( + np.array(prior.state_vector).reshape(-1), + np.diag([10, 0.1, 0.1, 10, 0.1, 0.1, 10, 0.1, 0.1, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5]), + size=number_particles) + +particles = [Particle(sample.reshape(-1, 1), + weight=Probability(1.)) + for sample in samples] + +# Particle prior +particle_prior = ParticleState(state_vector=None, + particle_list=particles, + timestamp=timestamps[0]) + +from stonesoup.types.track import Track +from stonesoup.types.hypothesis import SingleHypothesis + +# Instantiate the various tracks +track_ukf, track_ekf, track_pf = Track(), Track(), Track() + +# Loop over the measurement +updaters = [UKF_updater, UKF_updater, PF_updater] +predictors = [UKF_predictor, EKF_predictor, PF_predictor] +tracks = [track_ukf, track_ekf, track_pf] +priors = [prior, prior, particle_prior] + +# %% +# 3) Run the trackers and obtain the tracks; +# ------------------------------------------ +# We can run the various trackers and generate some tracks. +# Then, we evaluate the tracking results and perform a 1-to-1 comparison on the accuracy of the +# tracking algorithms using the RMSE. +# + +# Loop over the various trackers +for predictor, updater, track, prior in zip(predictors, updaters, tracks, priors): + for k, measurement in enumerate(measurements_set): + predictions = predictor.predict(prior, timestamp=measurement.timestamp) + hyps = SingleHypothesis(predictions, measurement) + post = updater.update(hyps) + track.append(post) + prior = track[-1] + +# Add the landmarks as fixed platforms +from stonesoup.platform.base import FixedPlatform + +platforms = [] +for target in targets: + state = np.array([target[0], 0, + target[1], 0, + target[2], 0]) + platforms.append( + FixedPlatform( + states=GaussianState(state, + np.diag([1,1,1,1,1,1]) + ), + position_mapping=(0, 2, 4) + )) + +from stonesoup.plotter import Plotter, Dimension + +plotter = Plotter(dimension=Dimension.THREE) +# Visualise with the landmarks +plotter.plot_ground_truths(groundtruths, mapping=[0, 3, 6]) +plotter.plot_tracks(track_ukf, mapping=[0, 3, 6], track_label='UKF') +plotter.plot_tracks(track_ekf, mapping=[0, 3, 6], track_label='EKF') +plotter.plot_tracks(track_pf, mapping=[0, 3, 6], track_label='PF') +plotter.plot_sensors({*platforms}, mapping=[0, 1, 2], + sensor_label='Landmarks') +plotter.fig + +# %% +# 4) Create and visualise the performances of the tracking algorithms +# ------------------------------------------------------------------- +# We have all the components from the tracking and now we can measure how well the +# various filter perform. We consider the RMSE (root mean square error) between the tracks and the +# groundtruth over the various simulation timesteps. +# + +pf_track, ekf_track, ukf_track, = np.zeros((1, simulation_steps)), \ + np.zeros((1, simulation_steps)), \ + np.zeros((1, simulation_steps)) + +for j in range(simulation_steps): + pf_track[:, j] = np.sqrt(((track_pf[j].state_vector[0].mean() - groundtruths[j].state_vector[0])**2. + + (track_pf[j].state_vector[3].mean() - groundtruths[j].state_vector[3])**2. + + (track_pf[j].state_vector[6].mean() - groundtruths[j].state_vector[6])**2.)/3.) + + ekf_track[:, j] = np.sqrt(((track_ekf[j].state_vector[0] - groundtruths[j].state_vector[0])**2. + + (track_ekf[j].state_vector[3] - groundtruths[j].state_vector[3])**2. + + (track_ekf[j].state_vector[6] - groundtruths[j].state_vector[6])**2.)/3.) + + ukf_track[:, j] = np.sqrt(((track_ukf[j].state_vector[0] - groundtruths[j].state_vector[0])**2. + + (track_ukf[j].state_vector[3] - groundtruths[j].state_vector[3])**2. + + (track_ukf[j].state_vector[6] - groundtruths[j].state_vector[6])**2.)/3.) + +plt.close() # close the previous figure +plt.plot(timesteps[0:simulation_steps], pf_track[0,:], color='red', linestyle='--', label='PF track') +plt.plot(timesteps[0:simulation_steps], ukf_track[0,:], color='blue', linestyle='--', label='UKF track') +plt.plot(timesteps[0:simulation_steps], ekf_track[0,:], color='orange', linestyle='--', label='EKF track') +plt.xlabel('Timesteps') +plt.ylabel('RMSE') +plt.title('Positional accuracy') +plt.legend() +plt.show() + +# %% +# Conclusion +# ---------- +# In this example we have compared the performances of some tracking algorithms available in +# Stone Soup providing measurements from the accelerometer and gyroscope on board of a +# sensor and using some landmarks on the ground to adjust the tracking. +# +# By adressing the RMSE (root mean square error) of the tracks obtained we can see that the Kalman +# Filter based algorithms offer good performances over the simulation, while the Particle filter +# seems to suffer from the high dimensionality of the problem. +# +# Overall, the intent of this example was to show how to use and perform a 1-to-1 comparison +# between these algorithms in Stone Soup. + +# sphinx_gallery_thumbnail_number = 1 \ No newline at end of file diff --git a/docs/examples/example_navigation.py b/docs/examples/example_navigation.py new file mode 100644 index 000000000..4650d96f2 --- /dev/null +++ b/docs/examples/example_navigation.py @@ -0,0 +1,374 @@ +#!/usr/bin/env python +# coding: utf-8 + +""" +========================================== +Example using navigation measurement model +========================================== +""" + +# %% +# 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, the +# 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 3D forces acting on the sensor. +# This example aims to provide an idea of how to use the combination of the measurement models +# :class:`~.AccelerometerMeasurementModel` and :class:`~.GyroscopeMeasurementModel` to model +# the inertia navigation measurements. +# In this example we ignore GPS measurements, therefore we employ the knowledge of fixed targets +# to adjust the navigation tracking from drifting, a common problem in navigation scenario. +# The state space we are considering is a 15 dimensions object, which combines 3D +# nearly-constant Acceleration model and the 3D Euler angles, whose are the heading ( +# :math:`\psi`), the pitch (:math:`\theta`) and the roll (:math:`\phi`) and their time derivative. +# +# This example follows these points: +# 1. Describe the transition model; +# 2. Obtain the ground truth and measurements; +# 3. Instantiate the tracker components; +# 4. Run the tracker and obtain the final track. +# + +# %% +# 1) Describe the transition model +# -------------------------------- +# As we have previously said, we want a 15 dimensions transition model for the sensor, +# in the simplest form we can combine :class:`~.ConstantAcceleration` and :class:`~.ConstantVelocity` +# transition model. Since the sensor is moving onto a fixed plane placed 1km above ground, we employ an +# exponential declining acceleration model, using :class:`~.Singer` model, to address +# the z- movements. A more complex, and realistic, approach would involve Van-Loan models for the transition. +# + +# %% +# General imports +# ^^^^^^^^^^^^^^^ +import numpy as np +from datetime import datetime, timedelta + +# %% +# Stone Soup and transition models +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +from stonesoup.types.groundtruth import GroundTruthState, GroundTruthPath +from stonesoup.types.detection import Detection +from stonesoup.types.state import State, StateVector, StateVectors, GaussianState +from stonesoup.models.transition.linear import CombinedGaussianTransitionModel, \ + ConstantVelocity, ConstantAcceleration, Singer +from stonesoup.functions.navigation import get_eulers_angles + +# %% +# Simulation parameters setup +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +# Lets assume a target sensor with these specifics +radius = 5000 # meters +speed = 200 # meters/seconds +center = np.array([0, 0, 1000]) # 3D center placed at 1km in height +n_timesteps = 100 + +timesteps = np.linspace(0, n_timesteps+1, n_timesteps+1) +simulation_start = datetime.now().replace(microsecond=0) +np.random.seed(2000) # fix a random seed for reproducibility + +# %% +# Describe the ground truth +# ^^^^^^^^^^^^^^^^^^^^^^^^^ +# In a different manner from other examples, we create the groundtruth of the sensor without considering +# the process noise, and at the same time, we calculate the sensor Euler angles. It is possible to still use the +# existing transition models and extend the state vectors to include such angles. +# + +# Create a function to create the groundtruth paths +def describe_sensor_motion(target_speed: float, + target_radius: float, + starting_position: np.array, + start_time: datetime, + number_of_timesteps: np.array + ) -> (list, set): + + """ + Auxiliary function to create the sensor-target dynamics in the + specific case of circular motion. + + Parameters: + ----------- + target_speed: float + Speed of the sensor; + target_radius: float + radius of the circular trajectory; + starting_position: np.array + starting point of the trajectory, latitude, longitude + and altitude; + start_time: datetime, + start of the simulation; + number_of_timesteps: np.array + simulation lenght + + Return: + ------- + (list, set): + list of timestamps of the simulation and + groundtruths path. + """ + + # Instantiate the 15 dimension object describing + # the positions, dynamics and angles of the target + sensor_dynamics = np.zeros((15)) + + # Generate the GroundTruthPath + truths = GroundTruthPath([]) + + # instantiate a list for the timestamps + timestamps = [] + + # indexes of the array + position_indexes = [0, 3, 6] + velocity_indexes = [1, 4, 7] + acceleration_indexes = [2, 5, 8] + angles_indexes = [9, 11, 13] + vangles_indexes = [10, 12, 14] + + sensor_dynamics[angles_indexes] + + # loop over the timestep + for i in number_of_timesteps: + theta = target_speed * i / target_radius + 0 + + # positions + sensor_dynamics[position_indexes] += target_radius * \ + np.array([np.cos(theta), np.sin(theta), + 0.001*np.random.choice(np.arange(-5, 5), 1)[0]]) + \ + starting_position + + # velocities + sensor_dynamics[velocity_indexes] += target_speed * \ + np.array([-np.sin(theta), np.cos(theta), 0]) + + # acceleration + sensor_dynamics[acceleration_indexes] += \ + ((-target_speed * target_speed) / target_radius) * np.array( + [np.cos(theta), np.sin(theta), 0]) + + # Now using the velocity and accelerations terms we get the Euler angles + angles, dangles = get_eulers_angles(sensor_dynamics[velocity_indexes], + sensor_dynamics[acceleration_indexes]) + + # add the Euler angles and their time derivative + # please check that are all angles + sensor_dynamics[angles_indexes] += angles + sensor_dynamics[vangles_indexes] += dangles + + # append all those as ground state + truths.append(GroundTruthState(state_vector=sensor_dynamics, + timestamp=start_time + timedelta(seconds=int(i)))) + # restart the array + sensor_dynamics = np.zeros((15)) + timestamps.append(start_time + timedelta(seconds=int(i))) + + return (timestamps, truths) + + +# Instantiate the transition model, We consider the Singer model for +# an exponential declining acceleration in the z- coordinate. +transition_model = CombinedGaussianTransitionModel([ConstantAcceleration(1.5), + ConstantAcceleration(1.5), + Singer(0.1, 10), + ConstantVelocity(0), + ConstantVelocity(0), + ConstantVelocity(0) + ]) + +# %% +# 2) Obtain the ground truth and gather the measurements; +# ------------------------------------------------------- +# We have instantiated a function to describe the target-sensor dynamics, obtaining the Euler angles +# from the vessel acceleration and velocity adopting the ad-hoc function :class:`~.get_euler_angles`. +# Likewise, we have instantiated the 15 dimension transition model using a constant acceleration +# model for the 3D dynamics and a constant velocity for modelling the Euler angles dynamics. +# +# We consider, as well, the :class:`~.Singer` model for an exponential declining acceleration +# 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 a +# :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 help to reduce the +# navigation drift. +# + +# %% +# Get the ground truth paths +# ^^^^^^^^^^^^^^^^^^^^^^^^^^ +# +timestamps, truths = describe_sensor_motion(speed, + radius, + center, + simulation_start, + timesteps) + +# %% +# Load and instantiate the measurement model +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +# We consider a case with the three fixed targets, landmarks, and we use the on-board +# measurements. To merge all these measurements we employ a :class:`~.CombinedReversibleGaussianMeasurementModel` +# to concatenate all the different measurement models. +# We specify a reference frame to evaluate the gravity forces applied onto the sensor, and it is needed for the +# accelerometer and gyroscope measurements. The landmarks are placed on the ground (z~0). +# Overall the measurement model will have 14 dimensions space. +# + +from stonesoup.models.measurement.nonlinear import AccelerometerMeasurementModel, \ + GyroscopeMeasurementModel, CartesianAzimuthElevationMeasurementModel, \ + CombinedReversibleGaussianMeasurementModel + +# Instantiate the measurement model +measurement_model_list = [] + +# Instantiate the landmarks - the z-coordinate is randomly drawn +target1 = np.array([3000, 3000, 0.0096]) +target2 = np.array([-3000, 3000, 1.6034]) +target3 = np.array([0, -3000, 0.93]) +target4 = np.array([0, 0, 1.5]) + +targets = [target1, target2, target3, target4] + +# Specify the reference frame for the Accelerometer +# and Gyroscope measurements. +reference_frame = StateVector([55, 0, 0]) # Latitude, longitude, Altitude + +accelerometer = AccelerometerMeasurementModel( + ndim_state=15, + mapping=(0, 3, 6), + noise_covar=np.diag([1, 1, 5]), # Acceleration + reference_frame=reference_frame +) + +gyroscope = GyroscopeMeasurementModel( + ndim_state=15, + mapping=(0, 3, 6), + noise_covar=np.diag([1e-7, 1e-7, 1e-7]), # Gyroscope + reference_frame=reference_frame +) + +# add the measurements models +measurement_model_list.append(accelerometer) +measurement_model_list.append(gyroscope) + +# loop over the various targets to initialise the +# azimuth-elevation models. +for target in targets: + measurement_model_list.append( + CartesianAzimuthElevationMeasurementModel( + ndim_state=15, + mapping=(0, 3, 6), + noise_covar=np.diag([1, 1]), + target_location=StateVector(target), + translation_offset=None) + ) + +# Combine all the measurement model into a unique +# model +measurement_model = CombinedReversibleGaussianMeasurementModel(measurement_model_list) + +# Now create the measurements +measurement_set = [] + +for truth in truths: + measurement = measurement_model.function(truth, noise=True) + measurement_set.append(Detection(state_vector=measurement, + timestamp=truth.timestamp, + measurement_model=measurement_model)) + + +# %% +# 3) instantiate the tracker components; +# -------------------------------------- +# We have the truths and the detections, in this simple example we do not include measurement clutter. +# Now we can set up the tracker components. +# +# In this example we consider an UnscentedKalmanFilter given the non-linearity of the problem. + +# %% +# Load the filter components +# ^^^^^^^^^^^^^^^^^^^^^^^^^^ +from stonesoup.predictor.kalman import UnscentedKalmanPredictor +from stonesoup.updater.kalman import UnscentedKalmanUpdater + +predictor = UnscentedKalmanPredictor(transition_model) +updater = UnscentedKalmanUpdater(None) + +# Covariance of the starting location +covar_starting_position = np.repeat(10, 15) + +# Instantiate the prior, with a known location of the sensor +prior = GaussianState( + state_vector=truths[0].state_vector, + covar=np.diag(covar_starting_position), + timestamp=timestamps[0] +) + +# %% +# 4) Run the tracker and obtain the final track. +# ---------------------------------------------- +# We have the tracker components and the starting (prior) knowledge, now we can loop over the +# various measurements and using a :class:`~.SingleHypothesis` we can perform the tracking. +# + +# Load these components to do the tracking +from stonesoup.types.track import Track +from stonesoup.types.hypothesis import SingleHypothesis + +track = Track() + +# Loop over the measurement +for k, measurement in enumerate(measurement_set): + predictions = predictor.predict(prior, timestamp=measurement.timestamp) + hyps = SingleHypothesis(predictions, measurement) + post = updater.update(hyps) + track.append(post) + prior = track[-1] + +# %% +# Load the plotter +# ^^^^^^^^^^^^^^^^ +# To plot the various landmarks we make use of the fixed platform object. + +from stonesoup.platform.base import FixedPlatform + +platforms = [] +for target in targets: + state = np.array([target[0], 0, + target[1], 0, + target[2], 0]) + platforms.append( + FixedPlatform( + states=GaussianState(state, + np.diag([1, 1, 1, 1, 1, 1])), + position_mapping=(0, 2, 4) + )) + +from stonesoup.plotter import Plotter, Dimension + +plotter = Plotter(dimension=Dimension.THREE) + +plotter.plot_ground_truths(truths, mapping=[0, 3, 6]) +plotter.plot_sensors({*platforms}, mapping=[0, 1, 2], + sensor_label='Landmarks') +plotter.plot_tracks(track, mapping=[0, 3, 6], uncertainty=False, track_label='Track') + +plotter.fig + +# %% +# Conclusion +# ---------- +# In this example we have shown how to use the inertia navigation functions and how to integrate the tracking using +# fixed landmarks. As it is evident from the tracking result this scenario is particularly complex +# and it is not possible to run a perfect track with the limited information available. +# Using different measurements for the landmarks, e.g. including the range between the target and sensor +# (i.e., see :class:`~.CartesianAzimuthElevationRangeMeasurementModel`), +# would improve the tracking. However this example aims to give an opportunity to show how to perform tracking +# in the inertia navigation context. +# diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 4f1a824b4..597e8d51b 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -2,6 +2,7 @@ import copy import numpy as np +import pymap3d from ..types.numeric import Probability from ..types.array import StateVector, StateVectors, CovarianceMatrix @@ -402,6 +403,68 @@ def sphere2cart(rho, phi, theta): return (x, y, z) +def sphere2GCS(x, y, z): + """Convert Cartesian coordinates to Latitude, Longitude and altitude + (Geographic Coordinate System), this function makes use of + pymap3d function ECEF2Geodetic. + ECEF (Earth centric, Earth Fixed Frame) is the usual reference frame + when considering the motion on objects on the Earth sphere. + The reference ellipsoid is WGS 84. + + Parameters + ---------- + x: float + The x coordinate in meters + y: float + the y coordinate in meters + z: float + the z coordinate in meters + + Returns + ------- + (degrees, degrees, float) + A tuple of the form `(latitude, longitude, altitude)` + """ + + latitude, longitude, altitude = pymap3d.ecef2geodetic(x, y, z) + + return (latitude, longitude, altitude) + + +def local_sphere2GCS(x_east, y_north, z_up, origin): + """Function similar to MATLAB local2latlong. + We pass the local x Easting, y Northing and + z altitude and a local reference point origin + and we compute the latitude, longitude and + altitude in the local reference frame. + + Parameters + ---------- + x_east: float + The x coordinate in meters + y_north: float + the y coordinate in meters + z_up: float + the z coordinate in meters + origin : Tuple + Local reference point + Returns + ------- + (float, float, float) + A tuple of the form `(latitude, longitude, altitude)` + """ + + # pass the reference frame point + lat0, lon0, alt0 = origin + + # The reference ellipsoid is WGS-84 + # we use the pymap3d enu2geodetic to obtain the results + latitude, longitude, altitude = pymap3d.enu2geodetic(x_east, y_north, z_up, + lat0, lon0, alt0) + + return (latitude, longitude, altitude) + + def cart2az_el_rg(x, y, z): """Convert Cartesian to azimuth (phi), elevation(theta), and range(rho) diff --git a/stonesoup/functions/navigation.py b/stonesoup/functions/navigation.py new file mode 100644 index 000000000..255cc7338 --- /dev/null +++ b/stonesoup/functions/navigation.py @@ -0,0 +1,390 @@ +""" +Navigation functions +-------------------- +""" + +import numpy as np +import pymap3d +from . import local_sphere2GCS, build_rotation_matrix + + +def earth_speed_flat_sq(dx, dy): + r"""Calculate the Earth speed flat vector respect to the reference + frame, from 2D Cartesian coordinates. + + Parameters + ---------- + dx : float, array like + :math:`dx` Earth velocity component + + dy : float, array like + :math:`dy` Earth velocity component + + Returns + ------- + np.array: sum of the powers of dx and dy + """ + + return np.power(dx, 2) + np.power(dy, 2) + + +def earth_speed_sq(dx, dy, dz): + r"""Calculate the Earth speed respect to the reference frame + from 3D Cartesian coordinates. + + Parameters + ---------- + dx : float, array like + :math:`dx` Earth velocity component + + dy : float, array like + :math:`dy` Earth velocity component + + dz : float, array like + :math:`dz` Earth velocity component + + Returns + ------- + np.array: sum of the powers of dx, dy and dz + """ + return np.power(dx, 2) + np.power(dy, 2) + np.power(dz, 2) + + +def earth_speed_flat(dx, dy): + r"""Same as :class:`~.earth_speed_flat_sq` but squared. + + Parameters + ---------- + dx : float, array like + :math:`dx` Earth velocity component + + dy : float, array like + :math:`dy` Earth velocity component + + Returns + ------- + np.array: square root of the sum of the powers of dx and dy + """ + return np.sqrt(earth_speed_flat_sq(dx, dy)) + + +def get_eulers_angles(earth_speed, earth_acceleration): + r"""Function to obtain the Euler angles from the + speed of the aeroplane. The Euler angles are: + + - :math:`\theta` : pitch (elevation) + - :math:`\phi` : roll (bank) + - :math:`\psi` : heading (yaw or Azimuth) + + the Euler angles are converted in radians to uniform with Stone Soup codebase + + Parameters + ---------- + earth_speed : np.array, float + Earth velocity components (dx, dy, dz) + in local Earth coordinates (m/s) + + earth_acceleration : np.array, float + Earth acceleration components (ddx, ddy, ddz) + in local Earth coordinates (m/s^2) + + Returns + ------- + np.array, float + a 3xN matrix of Euler angles (roll, pitch, heading) (in radians) + + np.array, float + a 3xN matrix of time derivatives of Euler angles (roll, pitch, heading) (radians/s) + + """ + + dx, dy, dz = earth_speed + ddx, ddy, ddz = earth_acceleration + + # Calculate the earth speed + Esfq = earth_speed_flat_sq(dx, dy) + Esf = earth_speed_flat(dx, dy) + Ess = earth_speed_sq(dx, dy, dz) + + Phi = np.arctan2(dy, dx) + Theta = np.arctan2(-dz, Esf) + Psi = np.radians(0.) + + composite_euler_angles = np.array([Psi, Theta, Phi]) + + composite_euler_acc_angles = np.array([0, 0, 0]) + + # in case of acceleration different from 0 + if earth_acceleration.any() > 0: + + num_dphi = dx*ddy - dy*ddx + + dPhi = num_dphi / Esfq + num_dtheta = (dx*ddz + dy*ddy)*dz/Esf - ddz*Esf + + dTheta = (num_dtheta / Ess) + dPsi = np.radians(0.) + + composite_euler_acc_angles = np.array([dPsi, dTheta, dPhi]) + + return (composite_euler_angles, composite_euler_acc_angles) + + +def euler2rotation_vector(psi_theta_phi, dpsi_theta_phi): + r""" Function to obtain the rotation vector for given Euler angles + and their time derivative. The Euler angles are + the heading of the plane, the pitch and roll. + The angles are in radians. + This function is taken from [#]_ + + Parameters + ---------- + psi_theta_phi: np.array, float + array containing the three Euler angles (radians) + + dpsi_theta_phi: np.array, float + array containing the time derivative of the + three Euler angles (radians/s) + + Returns + ------- + np.array, float + :math:`\omega_{deg}', array of the rotation vectors (radians/s) + + Reference + --------- + .. [#] P. Groves, Principles of GNSS, Inertial, + and Multisensor Integrated + Navigation Systems (Second Edition), Artech House, 2013. + """ + + phi_rad = psi_theta_phi[0, :] + theta_rad = psi_theta_phi[1, :] + + sin_theta = np.sin(theta_rad) + cos_theta = np.cos(theta_rad) + sin_phi = np.sin(phi_rad) + cos_phi = np.cos(phi_rad) + + R = np.array([[np.ones_like(theta_rad), np.zeros_like(theta_rad), -sin_theta], + [np.zeros_like(theta_rad), cos_phi, sin_phi * cos_theta], + [np.zeros_like(theta_rad), -sin_phi, cos_phi * cos_theta] + ]) + + return np.einsum('ijh, jh-> ih', R, dpsi_theta_phi) + + +def get_angular_rotation_vector(states, lat_lon_alt0, position_mapping=(0, 3, 6), + angles_mapping=(9, 11, 13), d_angles_mapping=(10, 12, 14)): + r"""Function to obtain the rotation vector measured by + the gyroscope instrument. + + Parameters + ---------- + states: :class:`~.State` + target state containing the positions, velocities, + acceleration and the Euler angles + lat_lon_alt0: np.array + reference frame in latitude, longitude and altitude + + position_mapping: tuple + indeces of the position mapping + angles_mapping: tuple + indeces of the angle mapping + d_angles_mapping: tuple + indeces of the angle derivative mapping + + Returns + ------- + np.array, float + :math:`\omega_{deg}' array of the rotation vectors (radians/s) + """ + + # Coordinates in navigation reference frame + localpos = states[position_mapping, :] # use the indices of the position + psi_theta_phi = states[angles_mapping, :] # use the indices of the Euler angles + dpsi_theta_phi = states[d_angles_mapping, :] # use the indices of the dEuler + + # Convert the local reference point to latitude and longitude + lat_deg, _, _ = local_sphere2GCS(localpos[0, :], + localpos[1, :], + localpos[2, :], + lat_lon_alt0) + + omega_nb_b = euler2rotation_vector(psi_theta_phi, dpsi_theta_phi) + + omega_ie_n = earth_turn_rate_vector(lat_deg[:]) + + Rbn = build_rotation_matrix(psi_theta_phi) + + omega_ib_b = ((Rbn @ omega_ie_n) + omega_nb_b) + + return omega_ib_b + + +def earth_turn_rate_vector(Lat_degs): + r"""Function to obtain the Earth turn rate vector + given the latitude in degrees. The Earth turn rate + is equal to :math:`7.29\times10^5` radians per seconds. + The equations are from [#]_. + + Parameters + ---------- + Lat_degs : float + latitude in degrees + + Returns + ------- + np.array + :math:`~\omega_{ie,i}` (radians/s) + + Reference + --------- + .. [#] M. Kok, J. Hol and T. Sch\"{o}n, + “Using Inertial Sensors for Position and Orientation Estimation”, + Foundations 456 and Trends in Signal Processing, Vol. 11, No. 1–2, pp 1–153, 2017. + """ + + # Earth turn rate (radians/s) + turn_rate = 7.292115e-5 + + # convert the latitude in radians for math operations + Lat_rads = np.radians(Lat_degs[:]) + + # Turn Rate of the Earth (omega_ie_n) + omega_ie_n = turn_rate * np.array([np.cos(Lat_rads), + np.zeros_like(Lat_rads), + -np.sin(Lat_rads)]) + return omega_ie_n + + +def get_force_vector(state, lat_lon_alt0, position_mapping=(0, 3, 6), + velocity_mapping=(1, 4, 7), acceleration_mapping=(2, 5, 8), + angles_mapping=(9, 11, 13)): + """ Measure the force measured by the accelerometer. + The final product is a matrix that measure + the forces as explained in [#]_. + + Parameters + ---------- + state : :class:`~.State` + Target states in 15-dimension space + + lat_lon_alt0: np.array + reference frame array in latitude, longitude and altitude + position_mapping: tuple + indeces of the position mapping + velocity_mapping: tuple + indeces of the velocity mapping + acceleration_mapping: tuple + indeces of the acceleration mapping + angles_mapping: tuple + indeces of the angle mapping + + Returns + ------- + np.array + 3D array containing the velocity components + of the Euler angles. + + Reference + --------- + .. [#] M. Kok, J. Hol and T. Sch\"{o}n, + “Using Inertial Sensors for Position and Orientation Estimation”, + Foundations 456 and Trends in Signal Processing, Vol. 11, No. 1–2, pp 1–153, 2017. + """ + + # coordinates in local navigation frame + localpos = state[position_mapping, :] + localvel = state[velocity_mapping, :] + localacc = state[acceleration_mapping, :] + psi_theta_phi = state[angles_mapping, :] + + lat_deg, _, alt = local_sphere2GCS(localpos[0, :], + localpos[1, :], + localpos[2, :], + lat_lon_alt0) + + # get the 3D local position, acceleration velocity + p_n = localpos + a_nn_n = localacc + v_n_n = localvel + + # omega rotation + omega_ie_n = earth_turn_rate_vector(lat_deg) + + a_ii_n = a_nn_n + 2 * np.cross(omega_ie_n, v_n_n, axis=0) + \ + np.cross(omega_ie_n, np.cross(omega_ie_n, p_n, axis=0), axis=0) + + grav_n = get_gravity_vector(lat_deg[:], alt[:]) + + Rbn = build_rotation_matrix(psi_theta_phi) + + fb = Rbn @ (a_ii_n - grav_n) + + return fb + + +def get_gravity_vector(lat_degs, alt): + """Obtain the gravity vector for a particular + latitude and altitude. + This code is adapted from [#]_. + + Parameters + ---------- + lat_degs : degrees + latitude in degrees + alt : float + altitude in meters + + Returns + ------- + g_vector : np.array, float + array containing the gravity + components on the specific latitude, altitude + location. + + Reference: + ---------- + .. [#] P. Groves, Principles of GNSS, Inertial, + and Multisensor Integrated Navigation Systems (Second Edition), Artech House, 2013. + """ + + # Define some WGS 84 ellipsoid + # earth_major_axis = pymap3d.Ellipsoid.from_name("wgs84").semimajor_axis # meters + earth_minor_axis = pymap3d.Ellipsoid.from_name("wgs84").semiminor_axis # meters + + # Get the Earth flattening and eccentricity + # flattening = (earth_major_axis - earth_minor_axis) / earth_major_axis + flattening = pymap3d.Ellipsoid.from_name("wgs84").flattening + + # eccentricity = np.sqrt(flattening * (2 - flattening)) + eccentricity = pymap3d.Ellipsoid.from_name("wgs84").eccentricity + + # Define the equatorial radius (adapted from Groves) + earth_radius = 6378137.0 # meters + + # Define the angular rate (omega_ie, WGS_84) + omega_i_e = 7.292115e-5 + + # Define the gravitational constant + mu = 3.9860044e14 + + # Convert the latitude in radians from degrees + lat_rads = np.radians(lat_degs[:]) + + # gravity model + g0_L = 9.7803253359 * ((1. + 0.001931853 * (np.sin(lat_rads)) ** 2) / + np.sqrt(1 - eccentricity ** 2 * (np.sin(lat_rads)) ** 2)) + + # gravity component north + g_north = -8.08e-9 * alt * np.sin(2 * lat_rads) + # gravity component low + g_down = g0_L * (1. - (2. / earth_radius) * + (1. + flattening * (1. - 2. * ((np.sin(lat_rads)) ** 2)) + + ((omega_i_e ** 2 * earth_radius ** 2 * earth_minor_axis) / mu)) + * alt + (3 / (earth_radius ** 2)) * alt ** 2) + + g_vector = np.array([g_north, np.zeros_like(g_down), g_down]) + + return g_vector diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index 2428a3490..1aae869e4 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -3,10 +3,12 @@ from numpy import deg2rad from scipy.linalg import cholesky, LinAlgError from pytest import approx, raises +from pymap3d import geodetic2enu from .. import ( cholesky_eps, jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma, - rotx, roty, rotz, cart2sphere, cart2angles, pol2cart, sphere2cart, dotproduct, gm_sample) + rotx, roty, rotz, cart2sphere, cart2angles, pol2cart, sphere2cart, dotproduct, gm_sample, + sphere2GCS, local_sphere2GCS) from ...types.array import StateVector, StateVectors, Matrix from ...types.state import State, GaussianState @@ -332,3 +334,74 @@ def test_gm_sample(means, covars, weights, size): assert samples.shape[0] == means[0].shape[0] else: assert samples.shape[0] == means.shape[0] + + +def test_sphere2GCS(): + """sphere2GCS test""" + + # Convert the various + def gps2ecef_custom(latitude, longitude, altitude): + # (lat, lon) in WSG-84 degrees + # altitude in meters + lat_rad = np.radians(latitude) + lon_rad = np.radians(longitude) + + # flattening + f = 1.0 / 298.257224 + # Earth Radius (meters) + R = 6378137. + cosLat = np.cos(lat_rad) + sinLat = np.sin(lat_rad) + + cosLong = np.cos(lon_rad) + sinLong = np.sin(lon_rad) + + c = 1 / np.sqrt(cosLat * cosLat + (1 - f) * (1 - f) * sinLat * sinLat) + s = (1 - f) * (1 - f) * c + + x = (R * c + altitude) * cosLat * cosLong + y = (R * c + altitude) * cosLat * sinLong + z = (R * s + altitude) * sinLat + + return x, y, z + + # list of cities + list_locations = [[51.507359, -0.136439], + [53.400002, -2.983333], + [-23.533773, -46.625290], + [35.652832, 139.839478], + [-35.282001, 149.128998]] + + for item in list_locations: + xyz2 = gps2ecef_custom(item[0], item[1], 0) + print(np.allclose(sphere2GCS(xyz2[0], xyz2[1], xyz2[2]), + np.array(item+[1e-5]), + rtol=1e-3, atol=1e-3)) # check if the tolerance if enough + + +def test_local_sphere2GCS(): + """local_sphere2GCS test""" + + # list of cities + list_locations = [[51.507359, -0.136439], + [53.400002, -2.983333], + [-23.533773, -46.625290], + [35.652832, 139.839478], + [-35.282001, 149.128998]] + # set a reference frame + reference_frame = np.array([0., 0., 0.]) + + # collect all the relevant xEasting, yNorthing and zUp given + # the reference frame + xyz = [] + for item in list_locations: + xyz.append(geodetic2enu(item[0], item[1], 1., + reference_frame[0], reference_frame[1], reference_frame[2])) + + for idx, ixyz in enumerate(xyz): + np.allclose(local_sphere2GCS(ixyz[0], ixyz[1], ixyz[2], + reference_frame), + np.array([list_locations[idx][0], + list_locations[idx][1], + 1.]), + rtol=1e-3) diff --git a/stonesoup/functions/tests/test_navigation.py b/stonesoup/functions/tests/test_navigation.py new file mode 100644 index 000000000..097ecd35a --- /dev/null +++ b/stonesoup/functions/tests/test_navigation.py @@ -0,0 +1,102 @@ +import pytest +import numpy as np + +from stonesoup.types.state import StateVector +from stonesoup.functions.navigation import earth_speed_flat_sq, earth_speed_sq, \ + earth_turn_rate_vector, get_gravity_vector, get_eulers_angles, get_force_vector, \ + get_angular_rotation_vector, euler2rotation_vector + + +@pytest.mark.parametrize( + "x, y, z", + [ # Cartesian values + (1., 0., 0.), + (0., 1., 0.), + (0., 0., 1.) + ] +) +def test_EarthSpeed(x, y, z): + """Speed respect the Earth tests""" + speed3D = np.power(x, 2) + np.power(y, 2) + np.power(z, 2) + speed2D = np.power(x, 2) + np.power(y, 2) + + # check that the 2D speed is the same as calculated with earth_speed_sq + assert np.allclose(speed3D, earth_speed_sq(x, y, z)) + + # check that the 3D speed is the same as calculated with EarthSpeed + assert np.allclose(speed2D, earth_speed_flat_sq(x, y)) + + +@pytest.mark.parametrize( + "latitude", + [ + np.array([40]), + np.array([55]), + np.array([60]), + np.array([10]), + np.array([35]) + ] +) +def test_earthTurnRateVector(latitude): + """earth_turn_rate_vector test""" + + turn_rate = 7.292115e-5 + + assert np.allclose(earth_turn_rate_vector(latitude), + turn_rate*np.array([ + np.cos(np.radians(latitude)), + np.zeros_like(latitude), + -np.sin(np.radians(latitude))] + )) + + +@pytest.mark.parametrize( + "latitude, altitude, gv", + [ + (np.array([55.0018]), np.array([1000]), + np.array([[-7.59254272e-06], [0.00000000e+00], [9.81199039e+00]])), + ] +) +def test_getGravityVector(latitude, altitude, gv): + """get_gravity_vector test""" + assert np.allclose(get_gravity_vector(latitude, altitude), gv, rtol=1e-4) + + +def test_functions_using_states(): + """A unique routine to test various + functions using a unique 15 dimension state + and check the results + """ + + state_test = StateVector( + [10, 20, 1, # xyz + 5, -5, 1, # v, xyz + 1, 1, 1, # a, xyz + np.radians(100), np.radians(2), # psi dpsi + np.radians(50), np.radians(5), # theta, dtheta + np.radians(0), np.radians(1)]) # phi, dpi + + # index state positions + speed_idx = [1, 4, 7] + acc_idx = [2, 5, 8] + ang_idx = [9, 11, 13] + vang_idx = [10, 12, 14] + + # latitude, longitude, altitude + reference = np.array([55, 0, 0]) + + # set of results + angular_rotation = np.array([[0.02151771], [-0.00417471], [-0.08787658]]) + + force_vector = np.array([[-6.11065211], [-6.50757451], [0.13454552]]) + + euler_rotation = np.array([[0.02153659], [-0.00410534], [-0.08788881]]) + + euler_angles = (np.array([0., -0.04846913, -0.24497866]), + np.array([0., -0.04668526, 0.05882353])) + + assert np.allclose(get_angular_rotation_vector(state_test, reference), angular_rotation) + assert np.allclose(get_force_vector(state_test, reference), force_vector) + assert np.allclose(euler2rotation_vector(state_test[ang_idx], state_test[vang_idx]), + euler_rotation) + assert np.allclose(get_eulers_angles(state_test[speed_idx], state_test[acc_idx]), euler_angles) diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index 3f45777b8..4d59fe40f 100644 --- a/stonesoup/models/measurement/nonlinear.py +++ b/stonesoup/models/measurement/nonlinear.py @@ -12,7 +12,8 @@ from ...functions import cart2pol, pol2cart, \ cart2sphere, sphere2cart, cart2angles, \ - build_rotation_matrix, cart2az_el_rg, az_el_rg2cart + build_rotation_matrix, cart2az_el_rg, az_el_rg2cart, mod_bearing +from ...functions.navigation import get_force_vector, get_angular_rotation_vector from ...types.array import StateVector, CovarianceMatrix, StateVectors from ...types.angle import Bearing, Elevation, Azimuth from ..base import LinearModel, GaussianModel, ReversibleModel @@ -1400,3 +1401,350 @@ def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]: out = super().rvs(num_samples, **kwargs) out = np.array([[Azimuth(0.)], [Elevation(0.)], [0.]]) + out return out + + +class AccelerometerMeasurementModel(NonLinearGaussianMeasurement): + r"""This is an implementation of the + Accelerometer measurement model, which models the + acceleration of the object, i.e. aircraft, in the + inertial frame resolved in the navigation frame + coordinates, including the gravitation forces and + the Earth relative movement. + This time varying model transforms the + 3D positional coordinates (x, y, z) with the + time derivative (velocity, dx, dy, dz) into measurements + of the accelerations applied to the object. + + Parameters + ---------- + class:`~.State` object comprised of the ground + truth model where we can extract the acceleration measurements. + + Returns + ------- + :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 3) + The model function evaluated given the provided time interval. + + Note + ---- + The current implementation of this class assumes a 3D Cartesian plane. + """ + + # We need a reference frame to measure the gravitational forces + reference_frame: StateVector = Property( + default=None, + doc="Reference frame in latitude, longitude and altitude") + + # velocity mapping + velocity_mapping: Tuple[int, int, int] = Property( + default=(1, 4, 7), + doc="Mapping to the targets velocity within its state space") + + # acceleration mapping + acceleration_mapping: Tuple[int, int, int] = Property( + default=(2, 5, 8), + doc="Mapping to the targets acceleration within its state space") + + # angle mapping + angle_mapping: Tuple[int, int, int] = Property( + default=(9, 11, 13), + doc="Mapping to the targets Euler angles within its state space") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + # Set values to defaults if not provided + if self.reference_frame is None: + self.reference_frame = StateVector([0, 0, 0]) + + @property + def ndim_meas(self): + return 3 + + def function(self, state, noise=False, **kwargs) -> StateVectors: + """Evaluate the measurements from the object state + """ + + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs(num_samples=state.state_vector.shape[1], **kwargs) + else: + noise = 0 + + acceleration_components = get_force_vector(state.state_vector, + self.reference_frame, + self.mapping, + self.velocity_mapping, + self.acceleration_mapping, + self.angle_mapping) + + return StateVectors(acceleration_components) + noise + + def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]: + out = super().rvs(num_samples, **kwargs) + out = np.array([[0], [0], [0]]) + out + return out + + +class GyroscopeMeasurementModel(NonLinearGaussianMeasurement): + r"""This is an implementation of the + Gyroscope measurement model. This model allows to + calculate the velocities of how the Euler angles of + the 3D object vary during motion. + The Euler angles are defined as heading (:math:`\psi`), + pitch (:math:`\theta`) and roll (:math:`\phi`). + + This model evaluates the acceleration of the + 3D object on the orientation of the angles. + + Parameters + ---------- + class:`~.State` object comprised of the ground + truth model where we can extract some measurements + + Returns + ------- + :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 3) + The model function evaluated given the provided time interval. + + Note + ---- + The current implementation of this class assumes a 3D Cartesian plane. + """ + + reference_frame: StateVector = Property( + default=None, + doc="Reference frame in latitude, longitude and altitude") + + # angle mapping + angle_mapping: Tuple[int, int, int] = Property( + default=(9, 11, 13), + doc="Mapping to the targets Euler angles within its state space") + # d angle mapping + d_angle_mapping: Tuple[int, int, int] = Property( + default=(10, 12, 14), + doc="Mapping to the targets time derivative Euler angles within its state space") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + # Set values to defaults if not provided + if self.reference_frame is None: + self.reference_frame = StateVector([0, 0, 0]) + + @property + def ndim_meas(self): + return 3 + + def function(self, state, noise=False, **kwargs) -> StateVectors: + """Evaluate the measurements from the object state + """ + + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs(num_samples=state.state_vector.shape[1], **kwargs) + else: + noise = 0 + + angles_components = get_angular_rotation_vector(state.state_vector, + self.reference_frame, + self.mapping, + self.angle_mapping, + self.d_angle_mapping) + + return StateVectors(angles_components) + noise + + def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]: + out = super().rvs(num_samples, **kwargs) + out = np.array([[0.], [0.], [0.]]) + out + return out + + +class CartesianAzimuthElevationMeasurementModel(NonLinearGaussianMeasurement): + + r"""This measurement model mimics the + Radio Frequency (RF) Sensing functionality and + data acquisition. This model provides information of + a target given the direction of motion and 3D components + of the observing sensor in terms of Azimuth and Elevation. + + Parameters + ---------- + class:`~.State` object comprised of the ground + truth model where we can extract some measurements + + Returns + ------- + :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 2) + The model function evaluated given the provided time interval. + + Note + ---- + The current implementation of this class assumes a 3D Cartesian plane. + """ + + target_location: StateVector = Property( + default=StateVector([0, 0, 0]), + doc=r"A 3x1 array specifying the Cartesian Target location in terms of :math:`x,y,z` " + "coordinates.") + + translation_offset: StateVector = Property( + default=None, + doc=r"A 3x1 array specifying the Cartesian origin offset in terms of :math:`x,y,z` " + "coordinates.") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + # Set values to defaults if not provided + if self.translation_offset is None: + self.translation_offset = StateVector([0] * 3) + if self.target_location is None: + # It should be given + self.translation_offset = StateVector([0] * 3) + + @property + def ndim_meas(self): + return 2 + + def function(self, state, + noise=False, + **kwargs) -> StateVectors: + + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs(num_samples=state.state_vector.shape[1], **kwargs) + else: + noise = 0 + + # adjust the sensor location with the translation offset, if present + sensor_location = state.state_vector[self.mapping, :] - self.translation_offset + + diff_position = self.target_location - sensor_location + + # evaluate the range + rg = np.linalg.norm(diff_position, axis=0) + + # evaluate the absolute azimuth and elevation of the target respect to the sensor + absolute_azimuth = np.arctan2(diff_position[1], diff_position[0]) + absolute_elevation = np.arcsin(diff_position[2] / rg) + + # evaluate the sensor heading and elevation + heading = state.state_vector[13, :] + pitch = state.state_vector[11, :] + + # Transform the azimuths and elevation and fix for 180 degrees in case + azimuths = [Azimuth(angle) for angle in mod_bearing(absolute_azimuth - heading)] + + elevations = [Elevation(angle) for angle in absolute_elevation - pitch] + + return StateVectors([azimuths, elevations]) + noise + + def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]: + out = super().rvs(num_samples, **kwargs) + out = np.array([[Azimuth(0.)], [Elevation(0.)]]) + out + return out + + +class CartesianAzimuthElevationRangeMeasurementModel(NonLinearGaussianMeasurement, + ReversibleModel): + r"""This measurement model mimics the + Radio Frequency (RF) Sensing functionality and + data acquisition. This model provides information of + a target given the direction of motion and 3D components + of the observing sensor in terms of Azimuth, Elevation and Range. + + The functionality of this measurement model is similar to + :class:`~.CartesianToAzimuthElevationRange`, but in this case + we incorporate the knowledge of the range between the + target and sensor. + + Parameters + ---------- + class:`~.State` object comprised of the ground + truth model where we can extract some measurements + + Returns + ------- + :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 2) + The model function evaluated given the provided time interval. + + Note + ---- + The current implementation of this class assumes a 3D Cartesian plane. + """ + + target_location: StateVector = Property( + default=StateVector([0, 0, 0]), + doc=r"A 3x1 array specifying the Cartesian Target location in terms of :math:`x,y,z` " + "coordinates.") + + translation_offset: StateVector = Property( + default=None, + doc=r"A 3x1 array specifying the Cartesian origin offset in terms of :math:`x,y,z` " + "coordinates.") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + # Set values to defaults if not provided + if self.translation_offset is None: + self.translation_offset = StateVector([0] * 3) + if self.target_location is None: + # It should be given + self.target_location = StateVector([0] * 3) + + @property + def ndim_meas(self): + return 3 + + def function(self, state, + noise=False, + **kwargs) -> StateVectors: + + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs(num_samples=state.state_vector.shape[1], **kwargs) + else: + noise = 0 + + # adjust the sensor location with the translation offset, if present + sensor_location = state.state_vector[self.mapping, :] - self.translation_offset + + diff_position = self.target_location - sensor_location + + # evaluate the range + rg = np.linalg.norm(diff_position, axis=0) + + # evaluate the absolute azimuth and elevation of the target respect to the sensor + absolute_azimuth = np.arctan2(diff_position[1], diff_position[0]) + absolute_elevation = np.arcsin(diff_position[2] / rg) + + # evaluate the sensor heading and elevation - if they are already in radians, avoid + heading = state.state_vector[13, :] + pitch = state.state_vector[11, :] + + # Transform the azimuths and elevation and fix for 180 degrees in case + azimuths = [Azimuth(angle) for angle in mod_bearing(absolute_azimuth - heading)] + elevations = [Elevation(angle) for angle in absolute_elevation - pitch] + + return StateVectors([azimuths, elevations, rg]) + noise + + def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]: + out = super().rvs(num_samples, **kwargs) + out = np.array([[Azimuth(0.)], [Elevation(0.)], [0.]]) + out + return out + + def inverse_function(self, detection, **kwargs) -> StateVector: + + az, el, range = detection.state_vector + + # define the output vector + out_vector = np.zeros((15, 1)).view(StateVector) + + z = np.sqrt(((range**2.)*(np.cos(az))**2.)/(1. + (np.cos(az))**2*(np.tan(el)**2.))) + x = z * np.tan(az) + y = z * np.tan(el) + + xyz = StateVectors([x, y, z]) + + # fill the output vector with x,y,z positions + out_vector[self.mapping, :] = xyz + self.translation_offset + + return out_vector diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py index 293a9f922..25a302a0e 100644 --- a/stonesoup/models/measurement/tests/test_models.py +++ b/stonesoup/models/measurement/tests/test_models.py @@ -8,13 +8,16 @@ CartesianToElevationBearingRange, CartesianToBearingRange, CartesianToElevationBearing, Cartesian2DToBearing, CartesianToBearingRangeRate, CartesianToElevationBearingRangeRate, RangeRangeRateBinning, - CartesianToAzimuthElevationRange) + CartesianToAzimuthElevationRange, CartesianAzimuthElevationRangeMeasurementModel, + CartesianAzimuthElevationMeasurementModel, GyroscopeMeasurementModel, + AccelerometerMeasurementModel) from ...base import ReversibleModel from ...measurement.linear import LinearGaussian from ....functions import jacobian as compute_jac from ....functions import pol2cart -from ....functions import rotz, rotx, roty, cart2sphere, cart2az_el_rg +from ....functions import rotz, rotx, roty, cart2sphere, cart2az_el_rg, mod_bearing +from ....functions.navigation import get_angular_rotation_vector, get_force_vector from ....types.angle import Bearing, Elevation, Azimuth from ....types.array import StateVector, StateVectors from ....types.state import State, CovarianceMatrix, ParticleState @@ -1390,3 +1393,294 @@ def test_models_with_particles(h, ModelClass, state_vec, R, - h(single_state_vec, model.mapping, model.translation_offset, model.rotation_offset) ).T, cov=R) + + +# Tests for inertia navigation and landmarks localisation +def h_az_el(state_vector, pos_map, target_state, translation_offset): + 'test the azimuth elevation measurement model' + # The target state behaves as a fixed landmark + sensor_location = state_vector[pos_map] - translation_offset + + diff_position = target_state - sensor_location + + # evaluate the range + rg = np.linalg.norm(diff_position, axis=0) + + # evaluate the absolute azimuth and elevation of the target respect to the sensor + absolute_azimuth = np.arctan2(diff_position[1], diff_position[0]) + absolute_elevation = np.arcsin(diff_position[2] / rg) + + # evaluate the sensor heading and elevation + heading = state_vector[13, :] + pitch = state_vector[11, :] + + # Transform the azimuths and elevation and fix for 180 degrees in case + azimuth = [Azimuth(angle) for angle in mod_bearing(absolute_azimuth - heading)] + elevation = [Elevation(angle) for angle in absolute_elevation - pitch] + + return StateVector([azimuth, elevation]) + + +def h_az_el_range(state_vector, pos_map, target_state, translation_offset): + ' test the 3D azimuth-elevation-range measurement model' + + sensor_location = state_vector[pos_map] - translation_offset + + diff_position = target_state - sensor_location + + # evaluate the range + rg = np.linalg.norm(diff_position, axis=0) + + # evaluate the absolute azimuth and elevation of the target respect to the sensor + absolute_azimuth = np.arctan2(diff_position[1], diff_position[0]) + absolute_elevation = np.arcsin(diff_position[2] / rg) + + # evaluate the sensor heading and elevation + heading = state_vector[13, :] + pitch = state_vector[11, :] + + # Transform the azimuths and elevation and fix for 180 degrees in case + azimuth = [Azimuth(angle) for angle in mod_bearing(absolute_azimuth - heading)] + elevation = [Elevation(angle) for angle in absolute_elevation - pitch] + + return StateVector([azimuth, elevation, rg]) + + +def h_accelerometer(state_vector, reference_frame): + ' test the accelerometer' + acceleration_components = get_force_vector(state_vector, + lat_lon_alt0=reference_frame) + return StateVectors(acceleration_components) + + +def h_gyroscope(state_vector, reference_frame): + ' test the gyroscope' + angles_components = get_angular_rotation_vector(state_vector, + lat_lon_alt0=reference_frame) + return StateVectors(angles_components) + + +@pytest.mark.parametrize( # Accelerometer and Gyroscope + "h, ModelClass, state_vec, mapping, R, \ + reference_frame", + [( # 3D meas, 15D state + h_accelerometer, + AccelerometerMeasurementModel, + StateVector([[5000, 0., -8.0, + 0., 200., 0., + 1000., 0., 0., + 90, 2.29, + 0.0, 0.0, + 0.0, 0.0]]), + np.array([0, 3, 6]), + np.array([1, 1, 1]), + np.array([55, 0, 0]) # reference frame + ), + ( # 3D meas, 15D state + h_gyroscope, + GyroscopeMeasurementModel, + StateVector([[5000, 0., -8.0, + 0., 200., 0., + 1000., 0., 0., + 90, 2.29, + 0.0, 0.0, + 0.0, 0.0]]), + np.array([0, 3, 6]), + np.array([1, 1, 1]), + np.array([55, 0, 0]) # reference frame + ) + ] +) +def test_models_sensor(h, ModelClass, state_vec, mapping, R, + reference_frame): + """ Test for the Accelerometer and Gyroscope Measurement Models """ + + ndim_state = state_vec.size + state = State(state_vec) + + model = ModelClass(ndim_state=ndim_state, + mapping=mapping, + noise_covar=np.diag(R), + reference_frame=reference_frame) + + R_flat = R.flat # Create flat 1-D array of R + with pytest.raises(ValueError, match="Covariance should have ndim of 2: got 1"): + ModelClass(ndim_state=ndim_state, + mapping=mapping, + noise_covar=R_flat, + reference_frame=reference_frame) + + # Project a state through the model + # (without noise) + meas_pred_wo_noise = model.function(state) + eval_m = h(state_vec, reference_frame) + assert np.array_equal(meas_pred_wo_noise, eval_m) + + # Ensure model creates noise + rvs = model.rvs() + assert rvs.shape == (model.ndim_meas, 1) + assert isinstance(rvs, StateVector) + rvs = model.rvs(10) + assert rvs.shape == (model.ndim_meas, 10) + assert isinstance(rvs, StateVectors) + assert not isinstance(rvs, StateVector) + + # Evaluate the likelihood of the predicted measurement, given the state + # (without noise) + prob = model.pdf(State(meas_pred_wo_noise), state) + assert approx(prob) == multivariate_normal.pdf( + (meas_pred_wo_noise + - np.array(h(state_vec, model.reference_frame)) + ).ravel(), + cov=np.diag(R)) + + # Propagate a state vector through the model + # (with internal noise) + meas_pred_w_inoise = model.function(state, noise=True) + assert not np.array_equal( + meas_pred_w_inoise, h(state_vec, model.reference_frame)) + + # Evaluate the likelihood of the predicted state, given the prior + # (with noise) + prob = model.pdf(State(meas_pred_w_inoise), state) + assert approx(prob) == multivariate_normal.pdf( + (meas_pred_w_inoise + - np.array(h(state_vec, model.reference_frame)) + ).ravel(), + cov=np.diag(R)) + + # Propagate a state vector through the model + # (with external noise) + noise = model.rvs() + meas_pred_w_enoise = model.function(state, + noise=noise) + assert np.array_equal(meas_pred_w_enoise, h( + state_vec, model.reference_frame)+noise) + + # Evaluate the likelihood of the predicted state, given the prior + # (with noise) + prob = model.pdf(State(meas_pred_w_enoise), state) + assert approx(prob) == multivariate_normal.pdf( + (meas_pred_w_enoise + - h(state_vec, model.reference_frame) + ).ravel(), + cov=np.diag(R)) + + +@pytest.mark.parametrize( # Landmarks + "h, ModelClass, state_vec, mapping, R, target_state," + "translation_offset", + [ + ( # 2D meas, 15D state + h_az_el, + CartesianAzimuthElevationMeasurementModel, + StateVector([[5000, 0., -8.0, + 0., 200., 0., + 1000., 0., 0., + np.radians(90), np.radians(2.29), + 0.0, 0.0, + 0.0, 0.0]]), + np.array([0, 3, 6]), # mapping + np.array([1, 1]), + StateVector([[1], [1], [1]]), # target state + StateVector([[0], [0], [0]]) # translation offset + ), + ( # 3D meas, 15D state + h_az_el_range, + CartesianAzimuthElevationRangeMeasurementModel, + StateVector([[5000, 0., -8.0, + 0., 200., 0., + 1000., 0., 0., + np.radians(90), np.radians(2.29), + 0.0, 0.0, + 0.0, 0.0]]), + np.array([0, 3, 6]), # mapping + np.array([1, 1, 1]), + StateVector([[1], [1], [1]]), # target state + StateVector([[0], [0], [0]]) # translation offset + )] +) +def test_models_landmarks(h, ModelClass, state_vec, mapping, R, + target_state, translation_offset): + """ Test for the Azimuth Elevation with Landmarks Measurement Models """ + + ndim_state = state_vec.size + state = State(state_vec) + + model = ModelClass(ndim_state=ndim_state, + mapping=mapping, + noise_covar=np.diag(R), + target_location=target_state, + translation_offset=translation_offset) + + R_flat = R.flat # Create flat 1-D array of R + with pytest.raises(ValueError, match="Covariance should have ndim of 2: got 1"): + ModelClass(ndim_state=ndim_state, + mapping=mapping, + noise_covar=R_flat, + target_location=target_state, + translation_offset=translation_offset) + + # Project a state through the model + # (without noise) + meas_pred_wo_noise = model.function(state) + eval_m = h(state_vec, mapping, target_state, translation_offset) + print(eval_m, eval_m.shape, meas_pred_wo_noise, meas_pred_wo_noise.shape) + assert np.array_equal(meas_pred_wo_noise, eval_m) + + # Ensure model creates noise + rvs = model.rvs() + assert rvs.shape == (model.ndim_meas, 1) + assert isinstance(rvs, StateVector) + rvs = model.rvs(10) + assert rvs.shape == (model.ndim_meas, 10) + assert isinstance(rvs, StateVectors) + assert not isinstance(rvs, StateVector) + + # Evaluate the likelihood of the predicted measurement, given the state + # (without noise) + prob = model.pdf(State(meas_pred_wo_noise), state) + assert approx(prob) == multivariate_normal.pdf( + (meas_pred_wo_noise + - np.array(h(state_vec, model.mapping, + model.target_location, model.translation_offset)) + ).ravel(), + cov=np.diag(R)) + + # Propagate a state vector through the model + # (with internal noise) + meas_pred_w_inoise = model.function(state, noise=True) + assert not np.array_equal( + meas_pred_w_inoise, h(state_vec, + model.mapping, + model.target_location, + model.translation_offset)) + + # Evaluate the likelihood of the predicted state, given the prior + # (with noise) + prob = model.pdf(State(meas_pred_w_inoise), state) + assert approx(prob) == multivariate_normal.pdf( + (meas_pred_w_inoise + - np.array(h(state_vec, model.mapping, + model.target_location, model.translation_offset)) + ).ravel(), + cov=np.diag(R)) + + # Propagate a state vector through the model + # (with external noise) + noise = model.rvs() + meas_pred_w_enoise = model.function(state, + noise=noise) + assert np.array_equal(meas_pred_w_enoise, + h(state_vec, model.mapping, + model.target_location, model.translation_offset)+noise) + + # Evaluate the likelihood of the predicted state, given the prior + # (with noise) + prob = model.pdf(State(meas_pred_w_enoise), state) + assert approx(prob) == multivariate_normal.pdf( + (meas_pred_w_enoise + - h(state_vec, model.mapping, + model.target_location, model.translation_offset) + ).ravel(), + cov=np.diag(R))