#
# Unit Test Script
# Module Name: pixelLineConverter.py
# Creation Date: May 16, 2019
# Author: Thibaud Teil
#
from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros
from Basilisk.fswAlgorithms import horizonOpNav
from Basilisk.utilities import RigidBodyKinematics as rbk
import os, inspect
import numpy as np
filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
def back_substitution(A, b):
n = b.size
x = np.zeros_like(b)
if A[-1, -1] == 0:
raise ValueError
x[-1] = b[-1]/ A[-1, -1]
for i in range(n-2, -1, -1):
sum=0
for j in range(i, n):
sum += A[i, j]*x[j]
x[i] = (b[i] - sum)/A[i,i]
return x
[docs]def test_horizonOpNav():
"""
Unit test for Horizon Navigation. The unit test specifically covers:
1. Individual methods: This module contains a back substitution method as well as a QR decomposition.
This test ensures that they are working properly with a direct test of the method input/outputs with
expected results
2. State and Covariances: This unit test also computes the state estimate and covariance in python. This is
compared directly to the output from the module for exact matching.
The Horizon Nav module gives the spacecraft position given a limb input. This test ensures that the results are as
expected both for the state estimate and the covariance associated with the measurement.
"""
[testResults, testMessage] = horizonOpNav_methods()
assert testResults < 1, testMessage
[testResults, testMessage] = horizonOpNav_update()
assert testResults < 1, testMessage
def horizonOpNav_methods():
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty array to store test log messages
###################################################################################
## Testing QR decomp
###################################################################################
Hinput = np.array([[1,2,3],[1,20,3],[3,0,1],[2,1,0],[20,-1, -5],[0,10,-5]])
numStates = np.shape(Hinput)[0]
# Fill in the variables for the test
Qin = horizonOpNav.new_doubleArray(3 * numStates)
Rin = horizonOpNav.new_doubleArray(3 * 3)
Hin = horizonOpNav.new_doubleArray(numStates * 3)
for j in range(numStates*3):
horizonOpNav.doubleArray_setitem(Qin, j, 0)
for j in range(3 * 3):
horizonOpNav.doubleArray_setitem(Rin, j, 0)
for j in range(numStates * 3):
horizonOpNav.doubleArray_setitem(Hin, j, Hinput.flatten().tolist()[j])
horizonOpNav.QRDecomp(Hin, numStates, Qin, Rin)
Qout = []
for j in range(3 * numStates):
Qout.append(horizonOpNav.doubleArray_getitem(Qin, j))
Rout = []
for j in range(3 * 3):
Rout.append(horizonOpNav.doubleArray_getitem(Rin, j))
q,r = np.linalg.qr(Hinput)
Rpy = np.zeros([3,3])
Qpy = np.zeros([numStates, 3])
for i in range(0,3):
Qpy[:,i] = Hinput[:,i]
for j in range(i):
Rpy[j,i] = np.dot(Qpy[:,j], Hinput[:,i])
Qpy[:,i]= Qpy[:,i] - Rpy[j,i]*Qpy[:,j]
Rpy[i,i] = np.linalg.norm(Qpy[:,i])
Qpy[:,i] = 1 / Rpy[i,i] * Qpy[:,i]
Qtest = np.array(Qout).reshape([numStates,3])
Rtest = np.array(Rout).reshape(3, 3)
errorNorm1 = np.linalg.norm(Qpy - Qtest)
errorNorm2 = np.linalg.norm(Rpy - Rtest)
if (errorNorm1 > 1.0E-10):
print(errorNorm1, "QR decomp")
testFailCount += 1
testMessages.append("QR decomp Failure in Q" + "\n")
if (errorNorm2 > 1.0E-10):
print(errorNorm2, "QR decomp")
testFailCount += 1
testMessages.append("QR decomp Failure in R" + "\n")
errorNorm1 = np.linalg.norm(q + Qtest)
errorNorm2 = np.linalg.norm(r[:3,:3] + Rtest)
if (errorNorm1 > 1.0E-10):
print(errorNorm1, "QR decomp")
testFailCount += 1
testMessages.append("QR decomp Failure in Q" + "\n")
if (errorNorm2 > 1.0E-10):
print(errorNorm2, "QR decomp")
testFailCount += 1
testMessages.append("QR decomp Failure in R" + "\n")
###################################################################################
## Testing Back Sub
###################################################################################
V = np.ones(3)
nIn = horizonOpNav.new_doubleArray(3)
VIn = horizonOpNav.new_doubleArray(3)
RIn = horizonOpNav.new_doubleArray(numStates*3)
for i in range(3):
horizonOpNav.doubleArray_setitem(nIn, i, 0.0)
for i in range(3*3):
horizonOpNav.doubleArray_setitem(RIn, i, r.flatten().tolist()[i])
for i in range(3):
horizonOpNav.doubleArray_setitem(VIn, i, V.flatten().tolist()[i])
horizonOpNav.BackSub(RIn, VIn, 3, nIn)
BackSubOut = []
for i in range(3):
BackSubOut.append(horizonOpNav.doubleArray_getitem(nIn, i))
exp = back_substitution(r[:3,:3], V)
BackSubOut = np.array(BackSubOut)
errorNorm = np.linalg.norm(exp - BackSubOut)
if(errorNorm > 1.0E-10):
print(errorNorm, "BackSub")
testFailCount += 1
testMessages.append("BackSub Failure " + "\n")
return [testFailCount, ''.join(testMessages)]
###################################################################################
## Testing dynamics matrix computation
###################################################################################
def horizonOpNav_update():
# Create a sim module as an empty container
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty array to store test log messages
unitTaskName = "unitTask" # arbitrary name (don't change)
unitProcessName = "TestProcess" # arbitrary name (don't change)
unitTestSim = SimulationBaseClass.SimBaseClass()
# This is needed if multiple unit test scripts are run
# This create a fresh and consistent simulation environment for each test run
# Create test thread
testProcessRate = macros.sec2nano(0.5) # update process rate update time
testProc = unitTestSim.CreateNewProcess(unitProcessName)
testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process
# Construct the ephemNavConverter module
# Set the names for the input messages
opNav = horizonOpNav.HorizonOpNavData() # Create a config struct
opNav.limbInMsgName = "limb_name"
opNav.cameraConfigMsgName = "camera_config_name"
opNav.attInMsgName = "nav_att_name"
opNav.noiseSF = 2
# ephemNavConfig.outputState = simFswInterfaceMessages.NavTransIntMsg()
# This calls the algContain to setup the selfInit, crossInit, update, and reset
opNavWrap = unitTestSim.setModelDataWrap(opNav)
opNavWrap.ModelTag = "limbNav"
# Add the module to the task
unitTestSim.AddModelToTask(unitTaskName, opNavWrap, opNav)
# These are example points for fitting used from an image processing algorithm
inputPoints = [226., 113., 227., 113., 223., 114., 224., 114., 225., 114., 219.,
115., 220., 115., 221., 115., 222., 115., 215., 116., 216., 116.,
217., 116., 218., 116., 212., 117., 213., 117., 214., 117., 209.,
118., 210., 118., 211., 118., 205., 119., 206., 119., 207., 119.,
208., 119., 204., 120., 205., 120., 201., 121., 202., 121., 203.,
121., 199., 122., 200., 122., 197., 123., 198., 123., 195., 124.,
196., 124., 193., 125., 194., 125., 191., 126., 192., 126., 189.,
127., 190., 127., 187., 128., 188., 128., 185., 129., 186., 129.,
183., 130., 184., 130., 181., 131., 182., 131., 180., 132., 181.,
132., 178., 133., 179., 133., 177., 134., 178., 134., 175., 135.,
176., 135., 174., 136., 175., 136., 172., 137., 173., 137., 171.,
138., 172., 138., 170., 139., 171., 139., 168., 140., 169., 140.,
167., 141., 168., 141., 166., 142., 167., 142., 164., 143., 165.,
143., 163., 144., 164., 144., 162., 145., 163., 145., 161., 146.,
162., 146., 160., 147., 161., 147., 159., 148., 160., 148., 158.,
149., 159., 149., 156., 150., 157., 150., 155., 151., 156., 151.,
154., 152., 155., 152., 153., 153., 154., 153., 153., 154., 152.,
155., 151., 156., 152., 156., 150., 157., 151., 157., 149., 158.,
150., 158., 148., 159., 149., 159., 147., 160., 148., 160., 146.,
161., 147., 161., 145., 162., 146., 162., 145., 163., 144., 164.,
143., 165., 144., 165., 142., 166., 143., 166., 142., 167., 141.,
168., 140., 169., 141., 169., 139., 170., 140., 170., 139., 171.,
138., 172., 137., 173., 138., 173., 137., 174., 136., 175., 135.,
176., 136., 176., 135., 177., 134., 178., 133., 179., 134., 179.,
133., 180., 132., 181., 132., 182., 131., 183., 131., 184., 130.,
185., 129., 186., 130., 186., 129., 187., 128., 188., 128., 189.,
127., 190., 127., 191., 126., 192., 126., 193., 125., 194., 125.,
195., 125., 196., 124., 197., 124., 198., 123., 199., 123., 200.,
122., 201., 122., 202., 122., 203., 121., 204., 120., 205., 121.,
205., 120., 206., 120., 207., 120., 208., 119., 209., 119., 210.,
119., 211., 118., 212., 118., 213., 118., 214., 117., 215., 117.,
216., 117., 217., 117., 218., 116., 219., 116., 220., 116., 221.,
116., 222., 115., 223., 115., 224., 115., 225., 115., 226., 114.,
227., 114., 228., 114., 229., 114., 230., 114., 231., 114., 232.,
113., 233., 113., 234., 113., 235., 113., 236., 113., 237., 113.,
238., 113., 239., 112., 240., 112., 241., 112., 242., 112., 243.,
112., 244., 112., 245., 112., 246., 112., 247., 112., 248., 112.,
249., 112., 250., 112., 251., 112., 252., 112., 253., 112., 254.,
111., 255., 111., 256., 112., 257., 112., 258., 112., 259., 112.,
260., 112., 261., 112., 262., 112., 263., 112., 264., 112., 265.,
112., 266., 112., 267., 112., 268., 112., 269., 112., 270., 112.,
271., 113., 272., 113., 273., 113., 274., 113., 275., 113., 276.,
113., 277., 113., 278., 114., 279., 114., 280., 114., 281., 114.,
282., 114., 283., 114., 284., 115., 285., 115., 286., 115., 287.,
115., 288., 116., 289., 116., 290., 116., 291., 116., 292., 117.,
293., 117., 294., 117., 295., 117., 296., 118., 297., 118., 298.,
118., 299., 119., 300., 119., 301., 119., 302., 120., 303., 120.,
304., 120., 305., 121., 306., 121., 307., 122., 308., 122., 309.,
122., 310., 123., 311., 123., 312., 124., 313., 124., 314., 125.,
315., 125., 316., 125., 317., 126., 318., 126., 319., 127., 320.,
127., 321., 128., 322., 128., 323., 129., 324., 129., 325., 130.,
325., 130., 326., 131., 327., 131., 328., 132., 329., 132., 330.,
133., 331., 133., 332., 134., 332., 134., 333., 135., 334., 135.,
335., 136., 335., 136., 336., 137., 337., 137., 338., 138., 338.,
138., 339., 139., 340., 139., 341., 140., 341., 140., 342., 141.,
342., 141., 343., 142., 344., 142., 345., 143., 345., 143., 346.,
144., 346., 144., 347., 145., 348., 145., 349., 146., 349., 146.,
350., 147., 350., 147., 351., 148., 351., 148., 352., 149., 352.,
149., 353., 150., 353., 150., 354., 151., 354., 151., 355., 152.,
356., 152., 357., 153., 357., 153., 358., 154., 358., 154., 359.,
155., 359., 155., 360., 156., 360., 156., 361., 157., 361., 158.,
362., 159., 362., 159., 363., 160., 363., 160., 364., 161., 364.,
161., 365., 162., 365., 162., 366., 163., 366., 163., 367., 164.,
367., 164., 368., 165., 368., 166., 369., 167., 369., 167., 370.,
168., 370., 168., 371., 169., 371., 169., 372., 170., 372., 171.,
373., 172., 373., 172., 374., 173., 374., 174., 375., 175., 375.,
175., 376., 176., 376., 177., 377., 178., 377., 178., 378., 179.,
378., 180., 379., 181., 379., 181., 380., 182., 380., 183., 381.,
184., 381., 185., 382., 186., 382., 187., 383., 188., 383., 188.,
384., 189., 384., 190., 385., 191., 385., 192., 386.]
# Create the input messages.
inputCamera = horizonOpNav.CameraConfigMsg()
inputLimbMsg = horizonOpNav.LimbOpNavMsg()
inputAtt = horizonOpNav.NavAttIntMsg()
# Set camera
inputCamera.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / 1. ) # 2*arctan(s/2 / f)
inputCamera.resolution = [512, 512]
inputCamera.sigma_CB = [1.,0.2,0.3]
unitTestSupport.setMessage(unitTestSim.TotalSim, unitProcessName, opNav.cameraConfigMsgName, inputCamera)
# Set circles
inputLimbMsg.valid = 1
inputLimbMsg.limbPoints = inputPoints
inputLimbMsg.numLimbPoints = int(len(inputPoints)/2)
inputLimbMsg.timeTag = 12345
unitTestSupport.setMessage(unitTestSim.TotalSim, unitProcessName, opNav.limbInMsgName, inputLimbMsg)
# Set attitude
inputAtt.sigma_BN = [0.6, 1., 0.1]
unitTestSupport.setMessage(unitTestSim.TotalSim, unitProcessName, opNav.attInMsgName, inputAtt)
# Set module for Mars
opNav.planetTarget = 2
opNav.opNavOutMsgName = "output_nav_msg"
unitTestSim.TotalSim.logThisMessage(opNav.opNavOutMsgName)
# Initialize the simulation
unitTestSim.InitializeSimulation()
# The result isn't going to change with more time. The module will continue to produce the same result
unitTestSim.ConfigureStopTime(testProcessRate) # seconds to stop simulation
unitTestSim.ExecuteSimulation()
# Truth Vlaues
############################
Q = np.eye(3)
B = np.zeros([3,3])
Q *= 1/(3396.19*1E3) # km
# Q[2,2] = 1/(3376.2*1E3)
numPoints = int(len(inputPoints)/2)
CB = rbk.MRP2C(inputCamera.sigma_CB)
BN = rbk.MRP2C(inputAtt.sigma_BN)
CN = np.dot(CB,BN)
B = np.dot(Q, CN.T)
# Transf camera to meters
alpha =0
up = inputCamera.resolution[0] / 2
vp = inputCamera.resolution[1] / 2
pX = 2. * np.tan(inputCamera.fieldOfView * inputCamera.resolution[0] / inputCamera.resolution[1] / 2.0)
pY = 2. * np.tan(inputCamera.fieldOfView / 2.0)
d_x = inputCamera.resolution[0] / pX
d_y = inputCamera.resolution[1] / pY
transf = np.zeros([3,3])
transf[0, 0] = 1 / d_x
transf[1, 1] = 1 / d_y
transf[2, 2] = 1
transf[0, 1] = -alpha/(d_x*d_y)
transf[0, 2] = (alpha*vp - d_y*up)/ (d_x * d_y)
transf[1, 2] = -vp / (d_y)
s = np.zeros([numPoints,3])
sBar = np.zeros([numPoints,3])
sBarPrime = np.zeros([numPoints,3])
H = np.zeros([numPoints,3])
for i in range(numPoints):
s[i,:] = np.dot(transf, np.array([inputPoints[2*i], inputPoints[2*i+1], 1]))
sBar[i,:] = np.dot(B, s[i,:])
sBarPrime[i,:] = sBar[i,:]/np.linalg.norm(sBar[i,:])
H[i,:] = sBarPrime[i,:]
# QR H
Rpy = np.zeros([3,3])
Qpy = np.zeros([numPoints, 3])
for i in range(0,3):
Qpy[:,i] = H[:,i]
for j in range(i):
Rpy[j,i] = np.dot(Qpy[:,j], H[:,i])
Qpy[:,i]= Qpy[:,i] - Rpy[j,i]*Qpy[:,j]
Rpy[i,i] = np.linalg.norm(Qpy[:,i])
Qpy[:,i] = 1 / Rpy[i,i] * Qpy[:,i]
errorNorm1 = np.linalg.norm(np.dot(Qpy, Rpy) - H)
if (errorNorm1 > 1.0E-8):
print(errorNorm1, "QR decomp")
testFailCount += 1
testMessages.append("QR decomp Failure in update test " + "\n")
# Back Sub
RHS = np.dot(Qpy.T, np.ones(numPoints))
n = back_substitution(Rpy, RHS)
n_test = np.dot(np.linalg.inv(Rpy), RHS)
R_s = (opNav.noiseSF*inputCamera.resolution[0]/(numPoints))**2/d_x**2*np.array([[1,0,0],[0,1,0],[0,0,0]])
R_s = np.dot(np.dot(B, R_s), B.T)
R_yInv = np.zeros([numPoints, numPoints])
for i in range(numPoints):
J = 1./np.linalg.norm(sBar[i,:])*np.dot(n, np.eye(3) - np.outer(sBarPrime[i,:], sBarPrime[i,:]))
temp = np.dot(R_s, J)
R_yInv[i,i] = 1./np.dot(temp, J)
Pn = np.linalg.inv(np.dot(np.dot(H.T, R_yInv),H))
F = -(np.dot(n,n) - 1)**(-0.5)*np.dot(np.linalg.inv(B), np.eye(3) - np.outer(n,n)/(np.dot(n,n)-1))
Covar_C_test = np.dot(np.dot(F, Pn), F.T)
errorNorm1 = np.linalg.norm(n_test - n)
if (errorNorm1 > 1.0E-8):
print(errorNorm1, "Back Sub")
testFailCount += 1
testMessages.append("Back Sub Failure in update test " + "\n")
r_BN_C = - (np.dot(n,n) - 1.)**(-0.5)*np.dot(np.linalg.inv(B), n)
posErr = 1e-3 #(m)
covarErr = 1e-5
unitTestSupport.writeTeXSnippet("toleranceValuePos", str(posErr), path)
unitTestSupport.writeTeXSnippet("toleranceValueVel", str(covarErr), path)
outputR = unitTestSim.pullMessageLogData(opNav.opNavOutMsgName + '.r_BN_C', list(range(3)))
outputCovar = unitTestSim.pullMessageLogData(opNav.opNavOutMsgName + '.covar_C', list(range(9)))
outputTime = unitTestSim.pullMessageLogData(opNav.opNavOutMsgName + '.timeTag')
for i in range(len(outputR[-1, 1:])):
if np.abs((r_BN_C[i] - outputR[0, i+1])/r_BN_C[i]) > posErr or np.isnan(outputR.any()):
testFailCount += 1
testMessages.append("FAILED: Position Check in Horizon Nav for index "+ str(i) + " with error " + str(np.abs((r_BN_C[i] - outputR[-1, i+1])/r_BN_C[i])))
for i in range(len(outputCovar[-1, 1:])):
if np.abs((Covar_C_test.flatten()[i] - outputCovar[0, i+1])/Covar_C_test.flatten()[i]) > covarErr or np.isnan(outputTime.any()):
testFailCount += 1
testMessages.append("FAILED: Covar Check in Horizon Nav for index "+ str(i) + " with error " + str(np.abs((Covar_C_test.flatten()[i] - outputCovar[-1, i+1]))))
snippentName = "passFail"
if testFailCount == 0:
colorText = 'ForestGreen'
print("PASSED: " + opNavWrap.ModelTag)
passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}'
else:
colorText = 'Red'
print("Failed: " + opNavWrap.ModelTag)
passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}'
print(testMessages)
unitTestSupport.writeTeXSnippet(snippentName, passedText, path)
return [testFailCount, ''.join(testMessages)]
if __name__ == '__main__':
# horizonOpNav_methods()
horizonOpNav_update()