Source code for test_inertialKF

#
#  ISC License
#
#  Copyright (c) 2016, 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 inspect
import math
import os

import matplotlib.pyplot as plt
import numpy
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import inertialUKF  # import the module that is to be tested
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import unitTestSupport  # general support file with common unit test functions

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
textSnippetPassed = r'\textcolor{ForestGreen}{' + "PASSED" + '}'
textSnippetFailed = r'\textcolor{Red}{' + "Failed" + '}'

def setupFilterData(filterObject):

    filterObject.alpha = 0.02
    filterObject.beta = 2.0
    filterObject.kappa = 0.0
    filterObject.switchMag = 1.2

    ST1Data = inertialUKF.STMessage()

    ST1Data.noise = [0.00017 * 0.00017, 0.0, 0.0,
                     0.0, 0.00017 * 0.00017, 0.0,
                     0.0, 0.0, 0.00017 * 0.00017]

    ST2Data = inertialUKF.STMessage()

    ST2Data.noise = [0.00017 * 0.00017, 0.0, 0.0,
                     0.0, 0.00017 * 0.00017, 0.0,
                     0.0, 0.0, 0.00017 * 0.00017]
    STList = [ST1Data, ST2Data]
    filterObject.STDatasStruct.STMessages = STList
    filterObject.STDatasStruct.numST = len(STList)

    filterObject.stateInit = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    filterObject.covarInit = [0.04, 0.0, 0.0, 0.0, 0.0, 0.0,
                              0.0, 0.04, 0.0, 0.0, 0.0, 0.0,
                              0.0, 0.0, 0.04, 0.0, 0.0, 0.0,
                              0.0, 0.0, 0.0, 0.004, 0.0, 0.0,
                              0.0, 0.0, 0.0, 0.0, 0.004, 0.0,
                              0.0, 0.0, 0.0, 0.0, 0.0, 0.004]
    qNoiseIn = numpy.identity(6)
    qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.0017*0.0017
    qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.00017*0.00017
    filterObject.qNoise = qNoiseIn.reshape(36).tolist()

    lpDataUse = inertialUKF.LowPassFilterData()
    lpDataUse.hStep = 0.5
    lpDataUse.omegCutoff = 15.0/(2.0*math.pi)
    filterObject.gyroFilt = [lpDataUse, lpDataUse, lpDataUse]

# 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 all_inertial_kfTest(show_plots): """Module Unit Tests""" # the following two tests appear to be broken # [testResults, testMessage] = statePropInertialAttitude(show_plots) # assert testResults < 1, testMessage # [testResults, testMessage] = statePropRateInertialAttitude(show_plots) # assert testResults < 1, testMessage [testResults, testMessage] = stateUpdateInertialAttitude(show_plots) assert testResults < 1, testMessage [testResults, testMessage] = stateUpdateRWInertialAttitude(show_plots) assert testResults < 1, testMessage [testResults, testMessage] = filterMethods() assert testResults < 1, testMessage
# [testResults, testMessage] = faultScenarios() # assert testResults < 1, testMessage def test_FilterMethods(): [testResults, testMessage] = filterMethods() assert testResults < 1, testMessage
[docs] def filterMethods(): """Module Unit Test""" testFailCount = 0 testMessages = [] 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(1.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) accuracy = 1E-10 # Construct algorithm and associated C++ container module = inertialUKF.inertialUKF() module.ModelTag = "inertialUKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) st1 = messaging.STAttMsgPayload() st1.timeTag = macros.sec2nano(1.25) st1.MRP_BdyInrtl = [0.1, 0.2, 0.3] st2 = messaging.STAttMsgPayload() st2.timeTag = macros.sec2nano(1.0) st2.MRP_BdyInrtl = [0.2, 0.2, 0.3] st3 = messaging.STAttMsgPayload() st3.timeTag = macros.sec2nano(0.75) st3.MRP_BdyInrtl = [0.3, 0.2, 0.3] ST1Data = inertialUKF.STMessage() ST2Data = inertialUKF.STMessage() ST3Data = inertialUKF.STMessage() STList = [ST1Data, ST2Data, ST3Data] state = inertialUKF.new_doubleArray(6) stateInput = numpy.array([1., 0., 0., 0.1, 0.1, 0.1]) for i in range(len(stateInput)): inertialUKF.doubleArray_setitem(state, i, stateInput[i]) wheelAccel = numpy.array([-5, 5]) / 1. * numpy.array([1., 1]) angAccel = -0.5 * (wheelAccel[0] + wheelAccel[1]) * numpy.array([1., 0., 0]) expectedRate = numpy.array(stateInput[3:]) + angAccel inertialUKF.inertialStateProp(module.getConfig(), state, 0.5) stateOut = [] for j in range(6): stateOut.append(inertialUKF.doubleArray_getitem(state, j)) if numpy.linalg.norm(expectedRate - numpy.array(stateOut)[3:]) > accuracy: testFailCount += 1 testMessages.append("Failed to capture wheel acceleration in inertialStateProp") setupFilterData(module) vehicleConfigOut = messaging.VehicleConfigMsgPayload() I = [1000., 0., 0., 0., 800., 0., 0., 0., 800.] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) module.STDatasStruct.STMessages = STList module.STDatasStruct.numST = len(STList) inertialUKFLog = module.logger("stSensorOrder") unitTestSim.AddModelToTask(unitTaskName, inertialUKFLog) # create ST input messages st1InMsg = messaging.STAttMsg().write(st1) st2InMsg = messaging.STAttMsg().write(st2) st3InMsg = messaging.STAttMsg().write(st3) # make input messages but don't write to them rwSpeedInMsg = messaging.RWSpeedMsg() rwConfigInMsg = messaging.RWArrayConfigMsg() gyroInMsg = messaging.AccDataMsg() # connect messages module.STDatasStruct.STMessages[0].stInMsg.subscribeTo(st1InMsg) module.STDatasStruct.STMessages[1].stInMsg.subscribeTo(st2InMsg) module.STDatasStruct.STMessages[2].stInMsg.subscribeTo(st3InMsg) module.massPropsInMsg.subscribeTo(vcInMsg) module.rwSpeedsInMsg.subscribeTo(rwSpeedInMsg) module.rwParamsInMsg.subscribeTo(rwConfigInMsg) module.gyrBuffInMsg.subscribeTo(gyroInMsg) # Star Tracker Read Message and Order method unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(1E9) unitTestSim.ExecuteSimulation() stOrdered = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.stSensorOrder) if numpy.linalg.norm(numpy.array(stOrdered[0]) - numpy.array([0., 2, 1, 0, 0])) > accuracy: testFailCount += 1 testMessages.append("ST order test failed") unitTestSupport.writeTeXSnippet("toleranceValue00", str(accuracy), path) if testFailCount == 0: print('Passed: test_FilterMethods') unitTestSupport.writeTeXSnippet("passFail00", textSnippetPassed, path) else: print('Failed: test_FilterMethods') unitTestSupport.writeTeXSnippet("passFail00", textSnippetFailed, path) return [testFailCount, ''.join(testMessages)]
def test_stateUpdateInertialAttitude(show_plots): [testResults, testMessage] = stateUpdateInertialAttitude(show_plots) assert testResults < 1, testMessage
[docs] def stateUpdateInertialAttitude(show_plots): """Module Unit Test""" # 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(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = inertialUKF.inertialUKF() module.ModelTag = "InertialUKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) module.maxTimeJump = 10 vehicleConfigOut = messaging.VehicleConfigMsgPayload() I = [1000., 0., 0., 0., 800., 0., 0., 0., 800.] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) stMessage1 = messaging.STAttMsgPayload() stMessage1.MRP_BdyInrtl = [0.3, 0.4, 0.5] st1InMsg = messaging.STAttMsg() stMessage2 = messaging.STAttMsgPayload() stMessage2.MRP_BdyInrtl = [0.3, 0.4, 0.5] st2InMsg = messaging.STAttMsg() # stateTarget = testVector.tolist() # stateTarget.extend([0.0, 0.0, 0.0]) # module.state = [0.7, 0.7, 0.0] inertialUKFLog = module.logger(["covar", "state"], testProcessRate*10) unitTestSim.AddModelToTask(unitTaskName, inertialUKFLog) # make input messages but don't write to them rwSpeedInMsg = messaging.RWSpeedMsg() rwConfigInMsg = messaging.RWArrayConfigMsg() gyroInMsg = messaging.AccDataMsg() # connect messages module.STDatasStruct.STMessages[0].stInMsg.subscribeTo(st1InMsg) module.STDatasStruct.STMessages[1].stInMsg.subscribeTo(st2InMsg) module.massPropsInMsg.subscribeTo(vcInMsg) module.rwSpeedsInMsg.subscribeTo(rwSpeedInMsg) module.rwParamsInMsg.subscribeTo(rwConfigInMsg) module.gyrBuffInMsg.subscribeTo(gyroInMsg) unitTestSim.InitializeSimulation() for i in range(20000): if i > 21: stMessage1.timeTag = int(i*0.5*1E9) stMessage2.timeTag = int(i*0.5*1E9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) unitTestSim.ExecuteSimulation() covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) accuracy = 1.0E-5 unitTestSupport.writeTeXSnippet("toleranceValue11", str(accuracy), path) for i in range(3): if(covarLog[-1, i*6+1+i] > covarLog[0, i*6+1+i]): testFailCount += 1 testMessages.append("Covariance update failure") unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) if(abs(stateLog[-1, i+1] - stMessage1.MRP_BdyInrtl[i]) > accuracy): print(abs(stateLog[-1, i+1] - stMessage1.MRP_BdyInrtl[i])) testFailCount += 1 testMessages.append("State update failure") unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) stMessage1.MRP_BdyInrtl = [1.2, 0.0, 0.0] stMessage2.MRP_BdyInrtl = [1.2, 0.0, 0.0] for i in range(20000): if i > 20: stMessage1.timeTag = int((i+20000)*0.25*1E9) stMessage2.timeTag = int((i+20000)*0.5*1E9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i+20000+1)*0.5)) unitTestSim.ExecuteSimulation() covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) for i in range(3): if(covarLog[-1, i*6+1+i] > covarLog[0, i*6+1+i]): testFailCount += 1 testMessages.append("Covariance update large failure") unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) plt.figure() for i in range(module.numStates): plt.plot(stateLog[:,0]*1.0E-9, stateLog[:,i+1], label='State_' +str(i)) plt.legend() plt.ylim([-1, 1]) unitTestSupport.writeFigureLaTeX('Test11', 'Test 1 State convergence', plt, 'width=0.9\\textwidth, keepaspectratio', path) plt.figure() for i in range(module.numStates): plt.plot(covarLog[:,0]*1.0E-9, covarLog[:,i*module.numStates+i+1], label='Covar_' +str(i)) plt.legend() plt.ylim([0, 2.E-7]) unitTestSupport.writeFigureLaTeX('Test12', 'Test 1 Covariance convergence', plt, 'width=0.9\\textwidth, keepaspectratio', path) if(show_plots): plt.show() plt.close('all') # print out success message if no error were found if testFailCount == 0: print('Passed: test_StateUpdateInertialAttitude') else: print('Failed: test_StateUpdateInertialAttitude') # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)]
def BROKENtest_statePropInertialAttitude(show_plots): [testResults, testMessage] = statePropInertialAttitude(show_plots) assert testResults < 1, testMessage
[docs] def statePropInertialAttitude(show_plots): """Module Unit Test""" # 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(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = inertialUKF.inertialUKF() module.ModelTag = "InertialUKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) vehicleConfigOut = messaging.VehicleConfigMsgPayload() I = [1000., 0., 0., 0., 800., 0., 0., 0., 800.] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) inertialUKFLog = module.logger(["covar", "state"], testProcessRate*10) # make input messages but don't write to them rwSpeedInMsg = messaging.RWSpeedMsg() rwConfigInMsg = messaging.RWArrayConfigMsg() gyroInMsg = messaging.AccDataMsg() st1InMsg = messaging.STAttMsg() st2InMsg = messaging.STAttMsg() # connect messages module.STDatasStruct.STMessages[0].stInMsg.subscribeTo(st1InMsg) module.STDatasStruct.STMessages[1].stInMsg.subscribeTo(st2InMsg) module.massPropsInMsg.subscribeTo(vcInMsg) module.rwSpeedsInMsg.subscribeTo(rwSpeedInMsg) module.rwParamsInMsg.subscribeTo(rwConfigInMsg) module.gyrBuffInMsg.subscribeTo(gyroInMsg) unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(8000.0)) unitTestSim.ExecuteSimulation() covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) accuracy = 1.0E-10 unitTestSupport.writeTeXSnippet("toleranceValue22", str(accuracy), path) for i in range(6): if(abs(stateLog[-1, i+1] - stateLog[0, i+1]) > accuracy): testFailCount += 1 testMessages.append("State propagation failure") unitTestSupport.writeTeXSnippet('passFail22', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail22', textSnippetPassed, path) for i in range(6): if(covarLog[-1, i*6+i+1] <= covarLog[0, i*6+i+1]): testFailCount += 1 testMessages.append("State covariance failure i="+str(i)) # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state propagation") else: print('Failed: test_StatePropInertialAttitude') print(testMessages) # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)]
def test_stateUpdateRWInertialAttitude(show_plots): [testResults, testMessage] = stateUpdateRWInertialAttitude(show_plots) assert testResults < 1, testMessage
[docs] def stateUpdateRWInertialAttitude(show_plots): """Module Unit Test""" # 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(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = inertialUKF.inertialUKF() module.ModelTag = "InertialUKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) vehicleConfigOut = messaging.VehicleConfigMsgPayload() I = [1000., 0., 0., 0., 800., 0., 0., 0., 800.] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) rwArrayConfigOut = messaging.RWArrayConfigMsgPayload() rwArrayConfigOut.numRW = 3 rwConfigInMsg = messaging.RWArrayConfigMsg().write(rwArrayConfigOut) rwSpeedIntMsg = messaging.RWSpeedMsgPayload() rwSpeedIntMsg.wheelSpeeds = [0.1, 0.01, 0.1] rwSpeedIntMsg.wheelThetas = [0.,0.,0.] rwSpeedInMsg = messaging.RWSpeedMsg().write(rwSpeedIntMsg) stMessage1 = messaging.STAttMsgPayload() stMessage1.MRP_BdyInrtl = [0.3, 0.4, 0.5] st1InMsg = messaging.STAttMsg() stMessage2 = messaging.STAttMsgPayload() stMessage2.MRP_BdyInrtl = [0.3, 0.4, 0.5] st2InMsg = messaging.STAttMsg() # stateTarget = testVector.tolist() # stateTarget.extend([0.0, 0.0, 0.0]) # module.state = [0.7, 0.7, 0.0] inertialUKFLog = module.logger(["covar", "state"], testProcessRate*10) unitTestSim.AddModelToTask(unitTaskName, inertialUKFLog) # make input messages but don't write to them gyroInMsg = messaging.AccDataMsg() # connect messages module.STDatasStruct.STMessages[0].stInMsg.subscribeTo(st1InMsg) module.STDatasStruct.STMessages[1].stInMsg.subscribeTo(st2InMsg) module.massPropsInMsg.subscribeTo(vcInMsg) module.rwSpeedsInMsg.subscribeTo(rwSpeedInMsg) module.rwParamsInMsg.subscribeTo(rwConfigInMsg) module.gyrBuffInMsg.subscribeTo(gyroInMsg) unitTestSim.InitializeSimulation() for i in range(20000): if i > 20: stMessage1.timeTag = int(i * 0.5 * 1E9) stMessage2.timeTag = int(i * 0.5 * 1E9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) if i==10000: rwSpeedIntMsg.wheelSpeeds = [0.5, 0.1, 0.05] rwSpeedInMsg.write(rwSpeedIntMsg, 0) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) print(inertialUKFLog.covar, covarLog) accuracy = 1.0E-5 unitTestSupport.writeTeXSnippet("toleranceValue33", str(accuracy), path) for i in range(3): if (covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]): testFailCount += 1 testMessages.append("Covariance update with RW failure") if (abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i]) > accuracy): print(abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i])) testFailCount += 1 testMessages.append("State update with RW failure") unitTestSupport.writeTeXSnippet('passFail33', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail33', textSnippetPassed, path) stMessage1.MRP_BdyInrtl = [1.2, 0.0, 0.0] stMessage2.MRP_BdyInrtl = [1.2, 0.0, 0.0] for i in range(20000): if i > 20: stMessage1.timeTag = int((i + 20000) * 0.25 * 1E9) stMessage2.timeTag = int((i + 20000) * 0.5 * 1E9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 20000 + 1) * 0.5)) unitTestSim.ExecuteSimulation() covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) for i in range(3): if (covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]): testFailCount += 1 testMessages.append("Covariance update large failure") unitTestSupport.writeTeXSnippet('passFail33', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail33', textSnippetPassed, path) plt.figure() for i in range(module.numStates): plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, i + 1], label='State_' +str(i)) plt.legend() plt.ylim([-1, 1]) unitTestSupport.writeFigureLaTeX('Test31', 'Test 3 State convergence', plt, 'width=0.7\\textwidth, keepaspectratio', path) plt.figure() for i in range(module.numStates): plt.plot(covarLog[:, 0] * 1.0E-9, covarLog[:, i * module.numStates + i + 1], label='Covar_' +str(i)) plt.legend() plt.ylim([0., 2E-7]) unitTestSupport.writeFigureLaTeX('Test32', 'Test 3 Covariance convergence', plt, 'width=0.7\\textwidth, keepaspectratio', path) if (show_plots): plt.show() plt.close('all') # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state update with RW") # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)]
def BROKENtest_StatePropRateInertialAttitude(show_plots): [testResults, testMessage] = statePropRateInertialAttitude(show_plots) assert testResults < 1, testMessage
[docs] def statePropRateInertialAttitude(show_plots): """Module Unit Test""" # 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(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = inertialUKF.inertialUKF() module.ModelTag = "InertialUKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) module.alpha = 0.02 module.beta = 2.0 module.kappa = 0.0 module.switchMag = 1.2 module.stateInit = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] module.covarInit = [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004] qNoiseIn = numpy.identity(6) qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.0017 * 0.0017 qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.00017 * 0.00017 module.qNoise = qNoiseIn.reshape(36).tolist() ST1Data = inertialUKF.STMessage() ST1Data.noise = [0.00017 * 0.00017, 0.0, 0.0, 0.0, 0.00017 * 0.00017, 0.0, 0.0, 0.0, 0.00017 * 0.00017] STList = [ST1Data] module.STDatasStruct.STMessages = STList module.STDatasStruct.numST = len(STList) lpDataUse = inertialUKF.LowPassFilterData() lpDataUse.hStep = 0.5 lpDataUse.omegCutoff = 15.0 / (2.0 * math.pi) module.gyroFilt = [lpDataUse, lpDataUse, lpDataUse] vehicleConfigOut = messaging.VehicleConfigMsgPayload() I = [1000., 0., 0., 0., 800., 0., 0., 0., 800.] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) stateInit = [0.0, 0.0, 0.0, math.pi/18.0, 0.0, 0.0] module.stateInit = stateInit inertialUKFLog = module.logger(["covar", "sigma_BNOut", "omega_BN_BOut"], testProcessRate*10) stMessage1 = messaging.STAttMsgPayload() stMessage1.MRP_BdyInrtl = [0., 0., 0.] stMessage1.timeTag = int(1* 1E9) st1InMsg = messaging.STAttMsg() # make input messages but don't write to them rwSpeedInMsg = messaging.RWSpeedMsg() rwConfigInMsg = messaging.RWArrayConfigMsg() gyroInMsg = messaging.AccDataMsg() # connect messages module.STDatasStruct.STMessages[0].stInMsg.subscribeTo(st1InMsg) module.massPropsInMsg.subscribeTo(vcInMsg) module.rwSpeedsInMsg.subscribeTo(rwSpeedInMsg) module.rwParamsInMsg.subscribeTo(rwConfigInMsg) module.gyrBuffInMsg.subscribeTo(gyroInMsg) unitTestSim.InitializeSimulation() st1InMsg.write(stMessage1, int(1 * 1E9)) gyroBufferData = messaging.AccDataMsgPayload() for i in range(3600*2+1): gyroBufferData.accPkts[i%inertialUKF.MAX_ACC_BUF_PKT].measTime = (int(i*0.5*1E9)) gyroBufferData.accPkts[i%inertialUKF.MAX_ACC_BUF_PKT].gyro_B = \ [math.pi/18.0, 0.0, 0.0] gyroInMsg.write(gyroBufferData, (int(i*0.5*1E9))) unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) unitTestSim.ExecuteSimulation() covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) sigmaLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.sigma_BNOut) omegaLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.omega_BN_BOut) accuracy = 1.0E-3 unitTestSupport.writeTeXSnippet("toleranceValue44", str(accuracy), path) for i in range(3): if(abs(omegaLog[-1, i+1] - stateInit[i+3]) > accuracy): print(abs(omegaLog[-1, i+1] - stateInit[i+3])) testFailCount += 1 testMessages.append("State omega propagation failure") unitTestSupport.writeTeXSnippet('passFail44', textSnippetFailed, path) else: unitTestSupport.writeTeXSnippet('passFail44', textSnippetPassed, path) for i in range(6): if(covarLog[-1, i*6+i+1] <= covarLog[0, i*6+i+1]): testFailCount += 1 testMessages.append("State covariance failure") # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state rate propagation") else: print("Failed: " + testMessages[0]) # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)]
def BROKENtest_FaultScenarios(show_plots): [testResults, testMessage] = faultScenarios(show_plots) assert testResults < 1, testMessage
[docs] def faultScenarios(show_plots): """Module Unit Test""" # 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(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Clean methods for Measurement and Time Updates moduleConfigClean1 = inertialUKF.InertialUKFConfig() moduleConfigClean1.numStates = 6 moduleConfigClean1.state = [0., 0., 0., 0., 0., 0.] moduleConfigClean1.statePrev = [0., 0., 0., 0., 0., 0.] moduleConfigClean1.sBar = [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] moduleConfigClean1.sBarPrev = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1.] moduleConfigClean1.covar = [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] moduleConfigClean1.covarPrev = [2., 0., 0., 0., 0., 0., 0., 2., 0., 0., 0., 0., 0., 0., 2., 0., 0., 0., 0., 0., 0., 2., 0., 0., 0., 0., 0., 0., 2., 0., 0., 0., 0., 0., 0., 2.] inertialUKF.inertialUKFCleanUpdate(moduleConfigClean1) if numpy.linalg.norm(numpy.array(moduleConfigClean1.covarPrev) - numpy.array(moduleConfigClean1.covar)) > 1E10: testFailCount += 1 testMessages.append("inertialUKFClean Covar failed") if numpy.linalg.norm(numpy.array(moduleConfigClean1.statePrev) - numpy.array(moduleConfigClean1.state)) > 1E10: testFailCount += 1 testMessages.append("inertialUKFClean States failed") if numpy.linalg.norm(numpy.array(moduleConfigClean1.sBar) - numpy.array(moduleConfigClean1.sBarPrev)) > 1E10: testFailCount += 1 testMessages.append("inertialUKFClean sBar failed") # inertialStateProp rate test with time step difference moduleConfigClean1.rwConfigParams.numRW = 2 moduleConfigClean1.rwSpeeds.wheelSpeeds = [10, 5] moduleConfigClean1.rwSpeedPrev.wheelSpeeds = [15, 10] moduleConfigClean1.rwConfigParams.JsList = [1., 1.] moduleConfigClean1.rwConfigParams.GsMatrix_B = [1., 0., 0., 1., 0., 0.] moduleConfigClean1.speedDt = 1. #moduleConfigClean1.IInv = [1., 0., 0., 0., 1., 0., 0., 0., 1.] # Bad Time and Measurement Update st1 = messaging.STAttMsgPayload() st1.timeTag = macros.sec2nano(1.) st1.MRP_BdyInrtl = [0.1, 0.2, 0.3] ST1Data = inertialUKF.STMessage() ST1Data.noise = [1., 0., 0., 0., 1., 0., 0., 0., 1.] STList = [ST1Data] moduleConfigClean1.alpha = 0.02 moduleConfigClean1.beta = 2.0 moduleConfigClean1.kappa = 0.0 moduleConfigClean1.switchMag = 1.2 moduleConfigClean1.countHalfSPs = moduleConfigClean1.numStates moduleConfigClean1.STDatasStruct.STMessages = STList moduleConfigClean1.STDatasStruct.numST = len(STList) moduleConfigClean1.wC = [-1] * (moduleConfigClean1.numStates * 2 + 1) moduleConfigClean1.wM = [-1] * (moduleConfigClean1.numStates * 2 + 1) retTime = inertialUKF.inertialUKFTimeUpdate(moduleConfigClean1, 1) retMease = inertialUKF.inertialUKFMeasUpdate(moduleConfigClean1, 1) if retTime == 0: testFailCount += 1 testMessages.append("Failed to catch bad Update and clean in Time update") if retMease == 0: testFailCount += 1 testMessages.append("Failed to catch bad Update and clean in Meas update") moduleConfigClean1.wC = [1] * (moduleConfigClean1.numStates * 2 + 1) moduleConfigClean1.wM = [1] * (moduleConfigClean1.numStates * 2 + 1) qNoiseIn = numpy.identity(6) qNoiseIn[0:3, 0:3] = -qNoiseIn[0:3, 0:3] * 0.0017 * 0.0017 qNoiseIn[3:6, 3:6] = -qNoiseIn[3:6, 3:6] * 0.00017 * 0.00017 moduleConfigClean1.qNoise = qNoiseIn.reshape(36).tolist() retTime = inertialUKF.inertialUKFTimeUpdate(moduleConfigClean1, 1) retMease = inertialUKF.inertialUKFMeasUpdate(moduleConfigClean1, 1) if retTime == 0: testFailCount += 1 testMessages.append("Failed to catch bad Update and clean in Time update") if retMease == 0: testFailCount += 1 testMessages.append("Failed to catch bad Update and clean in Meas update") # print out success message if no error were found if testFailCount == 0: print("PASSED: state rate propagation") else: print(testMessages) # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)]
if __name__ == "__main__": # filterMethods() # stateUpdateInertialAttitude(True) # statePropInertialAttitude(True) # Broken test # stateUpdateRWInertialAttitude(True) # statePropRateInertialAttitude(True) # faultScenarios(True) all_inertial_kfTest(True)