#
# ISC License
#
# Copyright (c) 2022, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#
r"""
Overview
--------
This scenario demonstrates how to use the ``smallBodyNavUKF()`` for translational state and non-Keplerian gravity
acceleration estimation about a small body. In this example, Vesta is chosen (``supportData/LocalGravData/VESTA20H.txt``).
However, any small body could be selected as long as the appropriate gravitational parameter is set.
In this scenario, :ref:`simpleNav` and :ref:`planetEphemeris` provide measurements to the UKF in the form of
:ref:`navTransMsgPayload` and :ref:`ephemerisMsgPayload` input messages. The UKF takes in these measurements at each
timestep and updates the state estimate, outputting this state estimate in its own standalone message, a
:ref:`smallBodyNavUKFMsgPayload`.
.. image:: /_images/static/test_scenarioSmallBodyNavUKF.svg
:align: center
.. note:: This module is only meant to demonstrate the possibility of estimating non-Keplerian acceleration in a small body environment. Therefore, realistic measurement modules do not exist to support this module, and not every source of uncertainty in the problem is an estimated parameter.
The relative position estimate and the estimation error and covariance may be found in the plots below.
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF3.svg
:align: center
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF6.svg
:align: center
The relative velocity estimate and the estimation error and covariance may be found in the plots below.
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF4.svg
:align: center
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF7.svg
:align: center
The non-Keplerian acceleration estimate (inhomogeneous gravity field) and the estimation error and covariance may be found in the plots below.
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF5.svg
:align: center
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF8.svg
:align: center
The spacecraft trajectories as seen from the inertial and asteroid fixed frame may be found in the plots below.
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF1.svg
:align: center
.. image:: /_images/Scenarios/scenarioSmallBodyNavUKF2.svg
:align: center
The script is found in the folder ``basilisk/examples`` and executed by using::
python3 scenarioSmallBodyNavUKF.py
"""
#
# Basilisk Scenario Script and Integrated Test
#
# Purpose: Example simulation to demonstrate the use of the smallBodyNavUKF
# Author: Julio C. Sanchez
# Creation Date: March 7th, 2022
#
import math
import os
import matplotlib.pyplot as plt
import numpy as np
from Basilisk.fswAlgorithms import smallBodyNavUKF
from Basilisk.simulation import ephemerisConverter
from Basilisk.simulation import planetEphemeris
from Basilisk.simulation import planetNav
from Basilisk.simulation import simpleNav
from Basilisk.simulation import spacecraft
from Basilisk.utilities import RigidBodyKinematics
from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody)
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import unitTestSupport
# The path to the location of Basilisk
# Used to get the location of supporting data.
fileName = os.path.basename(os.path.splitext(__file__)[0])
# Plotting functions
[docs]
def plot_3Dposition(r_truth, title='None'):
"""Plot the relative position in 3D."""
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1, projection='3d')
ax.plot(r_truth[:, 0] / 1000., r_truth[:, 1]/1000., r_truth[:,2]/1000., 'b')
if title == 'inertial':
ax.set_xlabel('${}^{N}r_{x}$ [km]')
ax.set_ylabel('${}^{N}r_{y}$ [km]')
ax.set_zlabel('${}^{N}r_{z}$ [km]')
ax.set_title('Inertial frame')
elif title == 'asteroid':
ax.set_xlabel('${}^{A}r_{x}$ [km]')
ax.set_ylabel('${}^{A}r_{y}$ [km]')
ax.set_zlabel('${}^{A}r_{z}$ [km]')
ax.set_title('Small body fixed frame')
[docs]
def plot_position(time, r_truth, r_est):
"""Plot the relative position result."""
fig, ax = plt.subplots(3, sharex=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time/(3600*24), r_truth[:, 0]/1000, 'b', label='truth')
ax[1].plot(time/(3600*24), r_truth[:, 1]/1000, 'b')
ax[2].plot(time/(3600*24), r_truth[:, 2]/1000, 'b')
ax[0].plot(time/(3600*24), r_est[:, 0]/1000, color='orange', label='estimate')
ax[1].plot(time/(3600*24), r_est[:, 1]/1000, color='orange')
ax[2].plot(time/(3600*24), r_est[:, 2]/1000, color='orange')
plt.xlabel('Time [days]')
plt.title('Spacecraft Position')
ax[0].set_ylabel('${}^{A}r_{x}$ [km]')
ax[1].set_ylabel('${}^{A}r_{y}$ [km]')
ax[2].set_ylabel('${}^{A}r_{z}$ [km]')
ax[0].legend()
return
[docs]
def plot_velocity(time, v_truth, v_est):
"""Plot the relative velocity result."""
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time/(3600*24), v_truth[:, 0], 'b', label='truth')
ax[1].plot(time/(3600*24), v_truth[:, 1], 'b')
ax[2].plot(time/(3600*24), v_truth[:, 2], 'b')
ax[0].plot(time/(3600*24), v_est[:, 0], color='orange', label='estimate')
ax[1].plot(time/(3600*24), v_est[:, 1], color='orange')
ax[2].plot(time/(3600*24), v_est[:, 2], color='orange')
plt.xlabel('Time [days]')
plt.title('Spacecraft Velocity')
ax[0].set_ylabel('${}^{A}v_{x}$ [m/s]')
ax[1].set_ylabel('${}^{A}v_{y}$ [m/s]')
ax[2].set_ylabel('${}^{A}v_{z}$ [m/s]')
ax[0].legend()
return
[docs]
def plot_acceleration(time, a_truth, a_est):
"""Plot the non-Keplerian acceleration result."""
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time/(3600*24), a_truth[:, 0]*1000, 'b', label='truth')
ax[1].plot(time/(3600*24), a_truth[:, 1]*1000, 'b')
ax[2].plot(time/(3600*24), a_truth[:, 2]*1000, 'b')
ax[0].plot(time/(3600*24), a_est[:, 0]*1000, color='orange', label='estimate')
ax[1].plot(time/(3600*24), a_est[:, 1]*1000, color='orange')
ax[2].plot(time/(3600*24), a_est[:, 2]*1000, color='orange')
plt.xlabel('Time [days]')
plt.title('Inhomogeneous gravity acceleration')
ax[0].set_ylabel('${}^{A}a_{x}$ [mm/s$^2$]')
ax[1].set_ylabel('${}^{A}a_{y}$ [mm/s$^2$]')
ax[2].set_ylabel('${}^{A}a_{z}$ [mm/s$^2$]')
ax[0].legend()
return
[docs]
def plot_pos_error(time, r_err, P):
"""Plot the position estimation error and associated covariance."""
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time/(3600*24), r_err[:, 0], 'b', label='error')
ax[0].plot(time/(3600*24), 2*np.sqrt(P[:, 0, 0]), 'k--', label=r'$2\sigma$')
ax[0].plot(time/(3600*24), -2*np.sqrt(P[:, 0, 0]), 'k--')
ax[1].plot(time/(3600*24), r_err[:, 1], 'b')
ax[1].plot(time/(3600*24), 2*np.sqrt(P[:, 1, 1]), 'k--')
ax[1].plot(time/(3600*24), -2*np.sqrt(P[:, 1, 1]), 'k--')
ax[2].plot(time/(3600*24), r_err[:, 2], 'b')
ax[2].plot(time/(3600*24), 2*np.sqrt(P[:, 2, 2]), 'k--')
ax[2].plot(time/(3600*24), -2*np.sqrt(P[:, 2, 2]), 'k--')
plt.xlabel('Time [days]')
plt.title('Position Error and Covariance')
ax[0].set_ylabel('${}^{A}r_{x}$ [m]')
ax[1].set_ylabel('${}^{A}r_{y}$ [m]')
ax[2].set_ylabel('${}^{A}r_{z}$ [m]')
ax[0].legend()
return
[docs]
def plot_vel_error(time, v_err, P):
"""Plot the velocity estimation error and associated covariance."""
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time/(3600*24), v_err[:, 0], 'b', label='error')
ax[0].plot(time/(3600*24), 2*np.sqrt(P[:, 3, 3]), 'k--', label=r'$2\sigma$')
ax[0].plot(time/(3600*24), -2*np.sqrt(P[:, 3, 3]), 'k--')
ax[1].plot(time/(3600*24), v_err[:, 1], 'b')
ax[1].plot(time/(3600*24), 2*np.sqrt(P[:, 4, 4]), 'k--')
ax[1].plot(time/(3600*24), -2*np.sqrt(P[:, 4, 4]), 'k--')
ax[2].plot(time/(3600*24), v_err[:, 2], 'b')
ax[2].plot(time/(3600*24), 2*np.sqrt(P[:, 5, 5]), 'k--')
ax[2].plot(time/(3600*24), -2*np.sqrt(P[:, 5, 5]), 'k--')
plt.xlabel('Time [days]')
plt.title('Velocity Error and Covariance')
ax[0].set_ylabel('${}^{A}v_{x}$ [m/s]')
ax[1].set_ylabel('${}^{A}v_{y}$ [m/s]')
ax[2].set_ylabel('${}^{A}v_{z}$ [m/s]')
ax[0].legend()
return
[docs]
def plot_acc_error(time, a_err, P):
"""Plot the non-Keplerian acceleration estimation error and associated covariance."""
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time/(3600*24), a_err[:, 0]*1000, 'b', label='error')
ax[0].plot(time/(3600*24), 2*np.sqrt(P[:, 6, 6])*1000, 'k--', label=r'$2\sigma$')
ax[0].plot(time/(3600*24), -2*np.sqrt(P[:, 6, 6])*1000, 'k--')
ax[1].plot(time/(3600*24), a_err[:, 1]*1000, 'b')
ax[1].plot(time/(3600*24), 2*np.sqrt(P[:, 7, 7])*1000, 'k--')
ax[1].plot(time/(3600*24), -2*np.sqrt(P[:, 7, 7])*1000, 'k--')
ax[2].plot(time/(3600*24), a_err[:, 2]*1000, 'b')
ax[2].plot(time/(3600*24), 2*np.sqrt(P[:, 8, 8])*1000, 'k--')
ax[2].plot(time/(3600*24), -2*np.sqrt(P[:, 8, 8])*1000, 'k--')
plt.xlabel('Time [days]')
plt.title('Acceleration Error and Covariance')
ax[0].set_ylabel('${}^{A}a_{x}$ [mm/s$^2$]')
ax[1].set_ylabel('${}^{A}a_{y}$ [mm/s$^2$]')
ax[2].set_ylabel('${}^{A}a_{z}$ [mm/s$^2$]')
ax[0].legend()
return
def run(show_plots):
from Basilisk import __path__
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])
# create simulation variable names
simTaskName = "simTask"
simProcessName = "simProcess"
# create a sim module as an empty container
scSim = SimulationBaseClass.SimBaseClass()
# create the simulation process
dynProcess = scSim.CreateNewProcess(simProcessName)
# create the dynamics task and specify the simulation time step information
simulationTimeStep = macros.sec2nano(15)
simulationTime = macros.sec2nano(4*24*3600.0)
dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
# setup celestial object ephemeris module
gravBodyEphem = planetEphemeris.PlanetEphemeris()
gravBodyEphem.ModelTag = 'vestaEphemeris'
gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["vesta"]))
# specify small body o.e. and rotational state January 21st, 2022
# https://ssd.jpl.nasa.gov/horizons.cgi#results
oeAsteroid = planetEphemeris.ClassicElements()
oeAsteroid.a = 2.3612 * orbitalMotion.AU * 1000 # meters
oeAsteroid.e = 0.08823
oeAsteroid.i = 7.1417*macros.D2R
oeAsteroid.Omega = 103.8*macros.D2R
oeAsteroid.omega = 151.1*macros.D2R
oeAsteroid.f = 7.0315*macros.D2R
# the rotational state would be prescribed to
AR = 309.03 * macros.D2R
dec = 42.23 * macros.D2R
lst0 = 0 * macros.D2R
gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid])
gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([AR])
gravBodyEphem.declination = planetEphemeris.DoubleVector([dec])
gravBodyEphem.lst0 = planetEphemeris.DoubleVector([lst0])
gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (5.3421 * 3600.)])
# initialize small body fixed frame dcm w.r.t. inertial and its angular velocity
dcm_AN = RigidBodyKinematics.euler3232C([AR , np.pi/2 - dec, lst0])
omega_AN_A = np.array([0,0,360 * macros.D2R / (5.3421 * 3600.)])
# setup small body gravity effector (no Sun 3rd perturbation included)
# https://ssd.jpl.nasa.gov/tools/gravity.html#/vesta
gravFactory = simIncludeGravBody.gravBodyFactory()
mu = 17.2882449693*1e9 # m^3/s^2
asteroid = gravFactory.createCustomGravObject("vesta", mu, radEquator=265*1000)
asteroid.isCentralBody = True
nSpherHarm = 14
asteroid.useSphericalHarmonicsGravityModel(bskPath + "/supportData/LocalGravData/VESTA20H.txt", nSpherHarm)
asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0])
# create an ephemeris converter
ephemConverter = ephemerisConverter.EphemerisConverter()
ephemConverter.ModelTag = "ephemConverter"
ephemConverter.addSpiceInputMsg(gravBodyEphem.planetOutMsgs[0])
# create SC object
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "bskSat"
gravFactory.addBodiesTo(scObject)
# setup orbit initial conditions of the sc
oe = orbitalMotion.ClassicElements()
oe.a = 500*1000 # meters
oe.e = 0.001
oe.i = 175 * macros.D2R
oe.Omega = 48.2 * macros.D2R
oe.omega = 347.8 * macros.D2R
oe.f = 85.3 * macros.D2R
r_CA_A, v_CA_A = orbitalMotion.elem2rv(mu, oe)
# set the truth ICs for the spacecraft position and velocity
scObject.hub.r_CN_NInit = dcm_AN.transpose().dot(r_CA_A)
scObject.hub.v_CN_NInit = dcm_AN.transpose().dot(v_CA_A)
# set up simpleNav for s/c "measurements"
simpleNavMeas = simpleNav.SimpleNav()
simpleNavMeas.ModelTag = 'SimpleNav'
simpleNavMeas.scStateInMsg.subscribeTo(scObject.scStateOutMsg)
pos_sigma_sc = 10.0
vel_sigma_sc = 0.1
att_sigma_sc = 0.0 * math.pi / 180.0
rate_sigma_sc = 0.0 * math.pi / 180.0
sun_sigma_sc = 0.0
dv_sigma_sc = 0.0
p_matrix_sc = [[pos_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., pos_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., pos_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., vel_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., vel_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., vel_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., att_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., att_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., att_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_sc, 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_sc, 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., sun_sigma_sc, 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., sun_sigma_sc, 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., sun_sigma_sc, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., dv_sigma_sc, 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., dv_sigma_sc, 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., dv_sigma_sc]]
walk_bounds_sc = [[10.], [10.], [10.], [0.1], [0.1], [0.1], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.]]
simpleNavMeas.PMatrix = p_matrix_sc
simpleNavMeas.walkBounds = walk_bounds_sc
# set up planetNav for small body measurements
planetNavMeas = planetNav.PlanetNav()
planetNavMeas.ephemerisInMsg.subscribeTo(ephemConverter.ephemOutMsgs[0])
# define the Pmatrix for planetNav, perfect state knowledge is assumed
pos_sigma_p = 0.0
vel_sigma_p = 0.0
att_sigma_p = 0 * math.pi / 180.0
rate_sigma_p = 0 * math.pi / 180.0
p_matrix_p = [[pos_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., pos_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., pos_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., vel_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., vel_sigma_p, 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., vel_sigma_p, 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., att_sigma_p, 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., att_sigma_p, 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., att_sigma_p, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_p, 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_p, 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_p]]
walk_bounds_p = [[0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.]]
planetNavMeas.PMatrix = p_matrix_p
planetNavMeas.walkBounds = walk_bounds_p
# set up the UKF
smallBodyNav = smallBodyNavUKF.SmallBodyNavUKF()
smallBodyNav.ModelTag = "smallBodyNavUKF"
# set the filter parameters (sc area, mass, gravitational constants, etc.)
smallBodyNav.mu_ast = mu # Gravitational constant of the asteroid
# set the process noise
P_proc_pos = 1000
P_proc_vel = 1
P_proc_acc = 1e-6
P_proc = [[P_proc_pos, 0., 0., 0., 0., 0., 0., 0., 0.],
[0., P_proc_pos, 0., 0., 0., 0., 0., 0., 0.],
[0., 0., P_proc_pos, 0., 0., 0., 0., 0., 0.],
[0., 0., 0., P_proc_vel, 0., 0., 0., 0., 0.],
[0., 0., 0., 0., P_proc_vel, 0., 0., 0., 0.],
[0., 0., 0., 0., 0., P_proc_vel, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., P_proc_acc, 0., 0.],
[0., 0., 0., 0., 0., 0., 0., P_proc_acc, 0.],
[0., 0., 0., 0., 0., 0., 0., 0., P_proc_acc]]
smallBodyNav.P_proc = P_proc
# set the measurement noise
R_meas = np.identity(3)
R_meas[0,0] = R_meas[1,1] = R_meas[2,2] = 100 # position sigmas
smallBodyNav.R_meas = R_meas.tolist() # Measurement Noise
# set the initial guess, x_0
x_0 = np.zeros(9)
x_0[0:3] = r_CA_A
x_0[3:6] = v_CA_A - np.cross(omega_AN_A,r_CA_A)
smallBodyNav.x_hat_k = x_0
# set the initial state covariance
P_k_pos = 1e4
P_k_vel = 0.1
P_k_acc = 1e-4
P_k = [[P_k_pos, 0., 0., 0., 0., 0., 0., 0., 0.],
[0., P_k_pos, 0., 0., 0., 0., 0., 0., 0.],
[0., 0., P_k_pos, 0., 0., 0., 0., 0., 0.],
[0., 0., 0., P_k_vel, 0., 0., 0., 0., 0.],
[0., 0., 0., 0., P_k_vel, 0., 0., 0., 0.],
[0., 0., 0., 0., 0., P_k_vel, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., P_k_acc, 0., 0.],
[0., 0., 0., 0., 0., 0., 0., P_k_acc, 0.],
[0., 0., 0., 0., 0., 0., 0., 0., P_k_acc]]
smallBodyNav.P_k = P_k
# set UKF hyperparameters
smallBodyNav.alpha = 0
smallBodyNav.beta = 2
smallBodyNav.kappa = 1e-3
# connect the relevant modules to the smallBodyUKF input messages
smallBodyNav.navTransInMsg.subscribeTo(simpleNavMeas.transOutMsg)
smallBodyNav.asteroidEphemerisInMsg.subscribeTo(planetNavMeas.ephemerisOutMsg)
# add all models to the task
scSim.AddModelToTask(simTaskName, gravBodyEphem, 100)
scSim.AddModelToTask(simTaskName, ephemConverter, 99)
scSim.AddModelToTask(simTaskName, scObject, 98)
scSim.AddModelToTask(simTaskName, simpleNavMeas, 97)
scSim.AddModelToTask(simTaskName, planetNavMeas, 96)
scSim.AddModelToTask(simTaskName, smallBodyNav, 95)
# setup data logging before the simulation is initialized
ast_ephem_recorder = gravBodyEphem.planetOutMsgs[0].recorder()
ast_truth_recorder = ephemConverter.ephemOutMsgs[0].recorder()
ast_meas_recorder = planetNavMeas.ephemerisOutMsg.recorder()
sc_truth_recorder = scObject.scStateOutMsg.recorder()
sc_nav_recorder = smallBodyNav.smallBodyNavUKFOutMsg.recorder()
sc_meas_recorder = simpleNavMeas.transOutMsg.recorder()
scSim.AddModelToTask(simTaskName, ast_ephem_recorder)
scSim.AddModelToTask(simTaskName, ast_truth_recorder)
scSim.AddModelToTask(simTaskName, ast_meas_recorder)
scSim.AddModelToTask(simTaskName, sc_truth_recorder)
scSim.AddModelToTask(simTaskName, sc_nav_recorder)
scSim.AddModelToTask(simTaskName, sc_meas_recorder)
# initialize Simulation
scSim.InitializeSimulation()
# configure a simulation stop time and execute the simulation run
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
# retrieve logged time and length
time = sc_truth_recorder.times() * macros.NANO2SEC
N_points = len(time)
# retrieve truth inertial position and velocity w.r.t small body
r_truth_N = np.array(sc_truth_recorder.r_BN_N - ast_truth_recorder.r_BdyZero_N)
v_truth_N = np.array(sc_truth_recorder.v_BN_N - ast_truth_recorder.v_BdyZero_N)
# retrieve small body rotational state
sigma_AN = ast_truth_recorder.sigma_BN
omega_AN_A = ast_truth_recorder.omega_BN_B
# initialize truth inertial position and velocity w.r.t. asteroid centered fixed frame
r_truth_A = np.zeros((N_points,3))
v_truth_A = np.zeros((N_points,3))
a_truth_A = np.zeros((N_points, 3))
# loop through simulation points
for ii in range(N_points):
# obtain rotation matrix
R_AN = RigidBodyKinematics.MRP2C(sigma_AN[ii][0:3])
# rotate position and velocity
r_truth_A[ii][0:3] = R_AN.dot(r_truth_N[ii][0:3])
v_truth_A[ii][0:3] = R_AN.dot(v_truth_N[ii][0:3]) - np.cross(omega_AN_A[ii][0:3], r_truth_A[ii][0:3])
# compute gravity acceleration and substract Keplerian term
a_truth_A[ii][0:3] = np.array(asteroid.spherHarm.computeField(r_truth_A[ii][0:3], nSpherHarm, False)).reshape(3)
# get filter output
x_hat = sc_nav_recorder.state
P = sc_nav_recorder.covar
# plot the results
figureList = {}
plot_3Dposition(r_truth_N, title='inertial')
pltName = fileName + "1"
figureList[pltName] = plt.figure(1)
plot_3Dposition(r_truth_A, title='asteroid')
pltName = fileName + "2"
figureList[pltName] = plt.figure(2)
plot_position(time, r_truth_A, x_hat[:,0:3])
pltName = fileName + "3"
figureList[pltName] = plt.figure(3)
plot_velocity(time, v_truth_A, x_hat[:,3:6])
pltName = fileName + "4"
figureList[pltName] = plt.figure(4)
plot_acceleration(time, a_truth_A, x_hat[:,6:9])
pltName = fileName + "5"
figureList[pltName] = plt.figure(5)
plot_pos_error(time, np.subtract(r_truth_A, x_hat[:,0:3]), P)
pltName = fileName + "6"
figureList[pltName] = plt.figure(6)
plot_vel_error(time, np.subtract(v_truth_A, x_hat[:,3:6]), P)
pltName = fileName + "7"
figureList[pltName] = plt.figure(7)
plot_acc_error(time, np.subtract(a_truth_A,x_hat[:,6:9]), P)
pltName = fileName + "8"
figureList[pltName] = plt.figure(8)
if show_plots:
plt.show()
# close the plots being saved off to avoid over-writing old and new figures
plt.close("all")
return figureList
#
# This statement below ensures that the unit test script can be run as a
# stand-along python script
#
if __name__ == "__main__":
run(
True # show_plots
)