Source code for test_pixelLineBiasUKF


# 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 pixelLineBiasUKF  # import the module that is to be tested
from Basilisk.utilities import RigidBodyKinematics as rbk
from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion

import pixelLineBias_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:6]
    dxdt[3:6] = -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
    filterObject.gamma = 0.9

    mu = 42828.314*1E9 #m^3/s^2
    elementsInit = orbitalMotion.ClassicElements()
    elementsInit.a = 8000*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)
    bias = [1,1,-2]

    filterObject.stateInit = r.tolist() + v.tolist() + bias
    filterObject.covarInit = [1000.*1E6, 0.0, 0.0, 0.0, 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, 0.0,
                              0.0, 0.0, 1000.*1E6, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 5*1E6, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0,
                              0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0,
                              0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0,]

    qNoiseIn = np.identity(9)
    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
    qNoiseIn[6:9, 6:9] = qNoiseIn[3:6, 3:6]*0.0001*0.0001
    filterObject.qNoise = qNoiseIn.reshape(9*9).tolist()

# 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
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, 1, 1, 1] covar = 10* np.eye(len(state)) dt = 10 mu = 42828.314 # Measurement Model Test data = pixelLineBiasUKF.PixelLineBiasUKFConfig() msg = messaging.OpNavCirclesMsgPayload() msg.circlesCenters = [100, 200] msg.circlesRadii = [100] msg.planetIds = [2] data.circlesInBuffer = msg data.planetId = 2 data.countHalfSPs = len(state) data.numStates = len(state) # Dynamics Model Test data.planetId = 2 stateIn = pixelLineBiasUKF.new_doubleArray(len(state)) for i in range(len(state)): pixelLineBiasUKF.doubleArray_setitem(stateIn, i, state[i]) pixelLineBiasUKF.relODStateProp(data, stateIn, dt) propedState = [] for i in range(len(state)): propedState.append(pixelLineBiasUKF.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") # Set up a measurement test data = pixelLineBiasUKF.PixelLineBiasUKFConfig() # Set up a circle input message msg = messaging.OpNavCirclesMsgPayload() msg.circlesCenters = [100, 200] msg.circlesRadii = [100] msg.planetIds = [2] data.circlesInBuffer = msg data.planetId = 2 data.countHalfSPs = len(state) data.numStates = len(state) # Set up attitud message att = messaging.NavAttMsgPayload() att.sigma_BN = [0, 0.2,-0.1] att.omega_BN_B = [0.,0.,0.] data.attInfo = att # Set up a camera message cam = messaging.CameraConfigMsgPayload() cam.sigma_CB = [-0.2, 0., 0.3] cam.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / (1.*1e-3) ) # 2*arctan(s/2 / f) cam.resolution = [512, 512] data.cameraSpecs = cam # Populate sigma points SP = np.zeros([len(state), 2*len(state) +1]) for i in range(2*len(state) + 1): if i ==0: SP[:, i] = np.array(state) if i < len(state) + 1 and i>0: SP[:,i] = np.array(state) + covar[:,i-1] if i > len(state): SP[:,i] = np.array(state) - covar[:,i-(len(state)+1)] data.SP = np.transpose(SP).flatten().tolist() data.state = state pixelLineBiasUKF.pixelLineBiasUKFMeasModel(data) yMeasOut = data.yMeas expectedMeas = np.zeros([6, 2*len(state)+1]) dcm_CB = rbk.MRP2C(cam.sigma_CB) dcm_BN = rbk.MRP2C(att.sigma_BN) dcm_CN = np.dot(dcm_CB, dcm_BN) pX = 2. * np.tan(cam.fieldOfView * cam.resolution[0] / cam.resolution[1] / 2.0) pY = 2. * np.tan(cam.fieldOfView / 2.0) X = pX / cam.resolution[0] Y = pY / cam.resolution[1] planetRad = 3396.19 obs = np.array([msg.circlesCenters[0], msg.circlesCenters[1], msg.circlesRadii[0], 0, 0, 0]) for i in range(2*len(state)+1): r_C = np.dot(dcm_CN, SP[0:3,i]) rNorm = np.linalg.norm(SP[0:3,i]) r_C = -1./r_C[2]*r_C centerX = r_C[0] / X centerY = r_C[1] / Y centerX += cam.resolution[0]/2 - 0.5 centerY += cam.resolution[1] / 2 - 0.5 rad = 1.0/X*np.tan(np.arcsin(planetRad/rNorm)) if i == 0: obs[3:5] = np.array(msg.circlesCenters[0:2]) - obs[0:2] obs[5] = rad - obs[2] for j in range(3): obs[3+j] = round(obs[3+j]) expectedMeas[0,i] = centerX - SP[6, i] expectedMeas[1,i] = centerY - SP[7, i] expectedMeas[2, i] = rad - SP[8, i] expectedMeas[3:, i] = SP[6:, i] yMeasTest = np.zeros([6, 2*len(state)+1]) for i in range(2*len(state)+1): yMeasTest[:,i] = yMeasOut[i*6:i*6+6] if np.linalg.norm((yMeasTest - expectedMeas))/np.linalg.norm(expectedMeas[:,0]) > 1.0E-15: testFailCount += 1 testMessages.append("State Prop Failure") if testFailCount == 0: print("PASSED: ") else: print(testMessages) 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 state = [250, 32000, 1000, 5, 3, 2, 1, 1, 1] 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 = pixelLineBiasUKF.pixelLineBiasUKF() module.ModelTag = "relodSuKF" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) # Create the input messages. inputCamera = messaging.CameraConfigMsgPayload() inputAtt = messaging.NavAttMsgPayload() # Set camera inputCamera.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / 0.01) # 2*arctan(s/2 / f) inputCamera.resolution = [512, 512] inputCamera.sigma_CB = [1., 0.3, 0.1] camInMsg = messaging.CameraConfigMsg().write(inputCamera) module.cameraConfigInMsg.subscribeTo(camInMsg) # Set attitude inputAtt.sigma_BN = [0.6, 1., 0.1] attInMsg = messaging.NavAttMsg().write(inputAtt) module.attInMsg.subscribeTo(attInMsg) circlesInMsg = messaging.OpNavCirclesMsg() module.circlesInMsg.subscribeTo(circlesInMsg) dataLog = module.filtDataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) timeSim = 60 unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.min2nano(timeSim)) unitTestSim.ExecuteSimulation() time = np.linspace(0, timeSim*60, (int) (timeSim*60/dt+1)) dydt = np.zeros(len(module.stateInit)) energy = np.zeros(len(time)) expected=np.zeros([len(time), len(module.stateInit)+1]) expected[0,1:] = module.stateInit mu = 42828.314*1E9 energy[0] = -mu/(2*orbitalMotion.rv2elem(mu, expected[0,1:4], expected[0,4:7]).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:7]).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") 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__": # relOD_method_test(True) StatePropRelOD(True, 1.0)