Source code for test_relativeODuKF


# ISC License
#
# Copyright (c) 2016-2018, 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.


import numpy as np
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import relativeODuKF  # import the module that is to be tested
from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion

import relativeODuKF_test_utilities as FilterPlots


def addTimeColumn(time, data):
    return np.transpose(np.vstack([[time], np.transpose(data)]))

def rk4(f, t, x0):
    x = np.zeros([len(t),len(x0)+1])
    h = (t[len(t)-1] - t[0])/len(t)
    x[0,0] = t[0]
    x[0,1:] = x0
    for i in range(len(t)-1):
        h = t[i+1] - t[i]
        x[i,0] = t[i]
        k1 = h * f(t[i], x[i,1:])
        k2 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k1)
        k3 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k2)
        k4 = h * f(t[i] + h, x[i,1:] + k3)
        x[i+1,1:] = x[i,1:] + (k1 + 2.*k2 + 2.*k3 + k4) / 6.
        x[i+1,0] = t[i+1]
    return x

def twoBodyGrav(t, x, mu = 42828.314*1E9):
    dxdt = np.zeros(np.shape(x))
    dxdt[0:3] = x[3:]
    dxdt[3:] = -mu/np.linalg.norm(x[0:3])**3.*x[0:3]
    return dxdt


def setupFilterData(filterObject):

    filterObject.planetIdInit = 2
    filterObject.alpha = 0.02
    filterObject.beta = 2.0
    filterObject.kappa = 0.0

    mu = 42828.314*1E9 #m^3/s^2
    elementsInit = orbitalMotion.ClassicElements()
    elementsInit.a = 4000*1E3 #m
    elementsInit.e = 0.2
    elementsInit.i = 10
    elementsInit.Omega = 0.001
    elementsInit.omega = 0.01
    elementsInit.f = 0.1
    r, v = orbitalMotion.elem2rv(mu, elementsInit)

    filterObject.stateInit = r.tolist() + v.tolist()
    filterObject.covarInit = [1000.*1E6, 0.0, 0.0, 0.0, 0.0, 0.0,
                              0.0, 1000.*1E6, 0.0, 0.0, 0.0, 0.0,
                              0.0, 0.0, 1000.*1E6, 0.0, 0.0, 0.0,
                              0.0, 0.0, 0.0, 5*1E6, 0.0, 0.0,
                              0.0, 0.0, 0.0, 0.0, 5*1E6, 0.0,
                              0.0, 0.0, 0.0, 0.0, 0.0, 5*1E6]

    qNoiseIn = np.identity(6)
    qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.00001*0.00001*1E-6
    qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.0001*0.0001*1E-6
    filterObject.qNoise = qNoiseIn.reshape(36).tolist()
    filterObject.noiseSF = 1

# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed
# @pytest.mark.skipif(conditionstring)
# uncomment this line if this test has an expected failure, adjust message as needed
# @pytest.mark.xfail() # need to update how the RW states are defined
# provide a unique test method name, starting with test_

[docs] def test_methods_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = relOD_method_test(show_plots) assert testResults < 1, testMessage
[docs] def test_propagation_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = StatePropRelOD(show_plots, 10.0) assert testResults < 1, testMessage
[docs] def test_measurements_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = StateUpdateRelOD(show_plots) assert testResults < 1, testMessage
def relOD_method_test(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages state = [250, 32000, 1000, 5, 3, 2] dt = 10 mu = 42828.314 # Measurement Model Test data = relativeODuKF.RelODuKFConfig() msg = messaging.OpNavMsgPayload() msg.r_BN_N = [300, 200, 100] data.planetId = 2 data.opNavInBuffer = msg data.countHalfSPs = 6 data.noiseSF = 1 Covar = np.eye(6) SPexp = np.zeros([6, 2*6+1]) SPexp[:,0] = np.array(state) for i in range(1, 6+1): SPexp[:,i] = np.array(state) + Covar[:,i-1] SPexp[:, i+6] = np.array(state) - Covar[:,i-1] data.SP = np.transpose(SPexp).flatten().tolist() relativeODuKF.relODuKFMeasModel(data) measurements = data.yMeas if np.linalg.norm(np.array(measurements) - np.transpose(SPexp[0:3,:]).flatten()) > 1.0E-15: testFailCount += 1 testMessages.append("Measurement Model Failure") # Dynamics Model Test data.planetId = 2 stateIn = relativeODuKF.new_doubleArray(6) for i in range(len(state)): relativeODuKF.doubleArray_setitem(stateIn, i, state[i]) relativeODuKF.relODStateProp(data, stateIn, dt) propedState = [] for i in range(6): propedState.append(relativeODuKF.doubleArray_getitem(stateIn, i)) expected = rk4(twoBodyGrav, [0, dt], np.array(state)*1E3) expected[:,1:]*=1E-3 if np.linalg.norm((np.array(propedState) - expected[-1,1:])/(expected[-1,1:])) > 1.0E-15: testFailCount += 1 testMessages.append("State Prop Failure") if testFailCount: print(testMessages) else: print("Passed") return [testFailCount, ''.join(testMessages)] def StateUpdateRelOD(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread dt = 1.0 t1 = 250 multT1 = 8 testProcessRate = macros.sec2nano(dt) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm module = relativeODuKF.relativeODuKF() module.ModelTag = "relodSuKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) module.noiseSF = 1 dataLog = module.filtDataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) time = np.linspace(0, int(multT1*t1), int(multT1*t1//dt)+1) dydt = np.zeros(6) energy = np.zeros(len(time)) expected=np.zeros([len(time), 7]) expected[0,1:] = module.stateInit mu = 42828.314*1E9 energy[0] = -mu/(2*orbitalMotion.rv2elem(mu, expected[0,1:4], expected[0,4:]).a) kick = np.array([0., 0., 0., -0.01, 0.01, 0.02]) * 10 *1E3 expected[0:t1,:] = rk4(twoBodyGrav, time[0:t1], module.stateInit) expected[t1:multT1*t1+1, :] = rk4(twoBodyGrav, time[t1:len(time)], expected[t1-1, 1:] + kick) for i in range(1, len(time)): energy[i] = - mu / (2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:]).a) inputData = messaging.OpNavMsgPayload() opnavInMsg = messaging.OpNavMsg() module.opNavInMsg.subscribeTo(opnavInMsg) inputData.planetID = 2 inputData.r_BN_B = expected[0, 1:4] opnavInMsg.write(inputData, 0) unitTestSim.InitializeSimulation() for i in range(t1): if i > 0 and i % 50 == 0: inputData.timeTag = macros.sec2nano(i * dt) inputData.r_BN_N = expected[i,1:4] + np.random.normal(0, 5*1E-2, 3) inputData.valid = 1 inputData.covar_N = [5.*1E-2, 0., 0., 0., 5.*1E-2, 0., 0., 0., 5.*1E-2] opnavInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * dt)) unitTestSim.ExecuteSimulation() covarLog = addTimeColumn(dataLog.times(), dataLog.covar) for i in range(6): if (covarLog[t1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100): testFailCount += 1 testMessages.append("Covariance update failure at " + str(t1)) for i in range(t1, multT1*t1): if i % 50 == 0: inputData.timeTag = macros.sec2nano(i * dt + 1) inputData.r_BN_N = expected[i,1:4] + np.random.normal(0, 5*1E-2, 3) inputData.valid = 1 inputData.covar_N = [5.*1E-2, 0.,0., 0., 5.*1E-2, 0., 0., 0., 5.*1E-2] opnavInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1)*dt)) unitTestSim.ExecuteSimulation() stateLog = addTimeColumn(dataLog.times(), dataLog.state) stateErrorLog = addTimeColumn(dataLog.times(), dataLog.stateError) postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) diff = np.copy(stateLog) diff[:,1:]-=expected[:,1:] FilterPlots.EnergyPlot(time, energy, 'Update', show_plots) FilterPlots.StateCovarPlot(stateLog, covarLog, 'Update', show_plots) FilterPlots.StatePlot(diff, 'Update', show_plots) FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4]) FilterPlots.PostFitResiduals(postFitLog, np.sqrt(5*1E-2*1E6), 'Update', show_plots) for i in range(6): if (covarLog[t1*multT1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100): testFailCount += 1 testMessages.append("Covariance update failure at " + str(t1*multT1)) if (np.linalg.norm(diff[-1, 1:]/expected[-1,1:]) > 1.0E-1): testFailCount += 1 testMessages.append("State propagation failure") # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state update") else: print(testMessages) # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)] def StatePropRelOD(show_plots, dt): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread testProcessRate = macros.sec2nano(dt) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = relativeODuKF.relativeODuKF() module.ModelTag = "relodSuKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) module.noiseSF = 1 dataLog = module.filtDataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) inputData = messaging.OpNavMsgPayload() opnavInMsg = messaging.OpNavMsg() module.opNavInMsg.subscribeTo(opnavInMsg) opnavInMsg.write(inputData, 0) timeSim = 60 unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.min2nano(timeSim)) unitTestSim.ExecuteSimulation() time = np.linspace(0, int(timeSim*60), int(timeSim*60//dt)+1) dydt = np.zeros(6) energy = np.zeros(len(time)) expected=np.zeros([len(time), 7]) expected[0,1:] = module.stateInit mu = 42828.314*1E9 energy[0] = -mu/(2*orbitalMotion.rv2elem(mu, expected[0,1:4], expected[0,4:]).a) expected = rk4(twoBodyGrav, time, module.stateInit) for i in range(1, len(time)): energy[i] = - mu / (2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:]).a) stateLog = addTimeColumn(dataLog.times(), dataLog.state) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) diff = np.copy(stateLog) diff[:,1:]-=expected[:,1:] FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4]) FilterPlots.EnergyPlot(time, energy, 'Prop', show_plots) FilterPlots.StateCovarPlot(stateLog, covarLog, 'Prop', show_plots) FilterPlots.StatePlot(diff, 'Prop', show_plots) if (np.linalg.norm(diff[-1,1:]/expected[-1,1:]) > 1.0E-10): testFailCount += 1 testMessages.append("State propagation failure") if (energy[0] - energy[-1])/energy[0] > 1.0E-10: testFailCount += 1 testMessages.append("State propagation failure") # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state propagation") # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)] if __name__ == "__main__": # relOD_method_test(True) # StatePropRelOD(True, 1.0) StateUpdateRelOD(False)