#
# 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.
#
# This test validates the EKF module by running several
# scenarios on both individual functions and the full module.
# Author: Thibaud Teil
import numpy as np
import pytest
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import sunlineEKF
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import unitTestSupport # general support file with common unit test functions
import SunLineEKF_test_utilities as FilterPlots
def addTimeColumn(time, data):
return np.transpose(np.vstack([[time], np.transpose(data)]))
def setupFilterData(filterObject):
filterObject.sensorUseThresh = 0.
filterObject.state = [1.0, 1.0, 1.0, 0.0, 0.0, 0.0]
filterObject.x = [1.0, 0.0, 1.0, 0.0, 0.1, 0.0]
filterObject.covar = [0.4, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.4, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 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]
filterObject.qProcVal = 0.1**2
filterObject.qObsVal = 0.001
filterObject.eKFSwitch = 5. #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF
[docs]
def test_all_functions_ekf(show_plots):
"""Module Unit Test"""
[testResults, testMessage] = sunline_individual_test()
assert testResults < 1, testMessage
[testResults, testMessage] = StatePropStatic()
assert testResults < 1, testMessage
[testResults, testMessage] = StatePropVariable(show_plots)
assert testResults < 1, testMessage
# 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(True)
# The following 'parametrize' function decorator provides the parameters and expected results for each
# of the multiple test runs for this test.
[docs]
@pytest.mark.parametrize("SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", [
(200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]),
(2000, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]),
(200, False ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]),
(200, False ,[0., 0.4, -0.4] ,[0., 0.7, 0.2], [0.3, 0.0, 0.6, 0.0, 0.0, 0.0]),
(200, True ,[0., 0.4, -0.4] ,[0.4, 0.5, 0.], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]),
(200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0])
])
# 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_
def test_all_sunline_ekf(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess):
"""Module Unit Test"""
[testResults, testMessage] = StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess)
assert testResults < 1, testMessage
def sunline_individual_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
###################################################################################
## Testing dynamics matrix computation
###################################################################################
inputStates = [2,1,0.75,0.1,0.4,0.05]
dt =0.5
expDynMat = np.zeros([6,6])
expDynMat[0:3, 0:3] = -(np.outer(inputStates[0:3],inputStates[3:6])/np.linalg.norm(inputStates[0:3])**2. +
np.dot(inputStates[3:6],inputStates[0:3])*(np.linalg.norm(inputStates[0:3])**2.*np.eye(3)- 2*np.outer(inputStates[0:3],inputStates[0:3]))/np.linalg.norm(inputStates[0:3])**4.)
expDynMat[0:3, 3:6] = np.eye(3) - np.outer(inputStates[0:3],inputStates[0:3])/np.linalg.norm(inputStates[0:3])**2
## Equations when removing the unobservable states from d_dot
expDynMat[3:6, 0:3] = -1/dt*(np.outer(inputStates[0:3],inputStates[3:6])/np.linalg.norm(inputStates[0:3])**2. +
np.dot(inputStates[3:6],inputStates[0:3])*(np.linalg.norm(inputStates[0:3])**2.*np.eye(3)- 2*np.outer(inputStates[0:3],inputStates[0:3]))/np.linalg.norm(inputStates[0:3])**4.)
expDynMat[3:6, 3:6] =- 1/dt*(np.outer(inputStates[0:3],inputStates[0:3])/np.linalg.norm(inputStates[0:3])**2)
dynMat = sunlineEKF.new_doubleArray(6*6)
for i in range(36):
sunlineEKF.doubleArray_setitem(dynMat, i, 0.0)
sunlineEKF.sunlineDynMatrix(inputStates, dt, dynMat)
DynOut = []
for i in range(36):
DynOut.append(sunlineEKF.doubleArray_getitem(dynMat, i))
DynOut = np.array(DynOut).reshape(6, 6)
errorNorm = np.linalg.norm(expDynMat - DynOut)
if(errorNorm > 1.0E-10):
print(errorNorm)
testFailCount += 1
testMessages.append("Dynamics Matrix generation Failure \n")
###################################################################################
## STM and State Test
###################################################################################
inputStates = [2,1,0.75, 1.5, 0.5, 0.5]
dt =0.5
stateTransition = sunlineEKF.new_doubleArray(36)
states = sunlineEKF.new_doubleArray(6)
for i in range(6):
sunlineEKF.doubleArray_setitem(states, i, inputStates[i])
for j in range(6):
if i==j:
sunlineEKF.doubleArray_setitem(stateTransition, 6*i+j, 1.0)
else:
sunlineEKF.doubleArray_setitem(stateTransition, 6*i+j, 0.0)
sunlineEKF.sunlineStateSTMProp(expDynMat.flatten().tolist(), dt, states, stateTransition)
PropStateOut = []
PropSTMOut = []
for i in range(6):
PropStateOut.append(sunlineEKF.doubleArray_getitem(states, i))
for i in range(36):
PropSTMOut.append(sunlineEKF.doubleArray_getitem(stateTransition, i))
STMout = np.array(PropSTMOut).reshape([6,6])
StatesOut = np.array(PropStateOut)
expectedSTM = dt*np.dot(expDynMat, np.eye(6)) + np.eye(6)
expectedStates = np.zeros(6)
inputStatesArray = np.array(inputStates)
## Equations when removing the unobservable states from d_dot
expectedStates[3:6] = np.array(inputStatesArray[3:6] - np.dot(inputStatesArray[3:6], inputStatesArray[0:3])*inputStatesArray[0:3]/np.linalg.norm(inputStatesArray[0:3])**2.)
expectedStates[0:3] = np.array(inputStatesArray[0:3] + dt*(inputStatesArray[3:6] - np.dot(inputStatesArray[3:6], inputStatesArray[0:3])*inputStatesArray[0:3]/np.linalg.norm(inputStatesArray[0:3])**2.))
errorNormSTM = np.linalg.norm(expectedSTM - STMout)
errorNormStates = np.linalg.norm(expectedStates - StatesOut)
if(errorNormSTM > 1.0E-10):
testFailCount += 1
testMessages.append("STM Propagation Failure \n")
if(errorNormStates > 1.0E-10):
testFailCount += 1
testMessages.append("State Propagation Failure \n")
###################################################################################
## Test the H and yMeas matrix generation as well as the observation count
###################################################################################
numCSS = 4
cssCos = [np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)), np.cos(np.deg2rad(90.))]
sensorTresh = np.cos(np.deg2rad(50.))
cssNormals = [1.,0.,0.,0.,1.,0., 0.,0.,1., 1./np.sqrt(2), 1./np.sqrt(2),0.]
cssBias = [1.0 for i in range(numCSS)]
measMat = sunlineEKF.new_doubleArray(8*6)
obs = sunlineEKF.new_doubleArray(8)
yMeas = sunlineEKF.new_doubleArray(8)
numObs = sunlineEKF.new_intArray(1)
for i in range(8*6):
sunlineEKF.doubleArray_setitem(measMat, i, 0.)
for i in range(8):
sunlineEKF.doubleArray_setitem(obs, i, 0.0)
sunlineEKF.doubleArray_setitem(yMeas, i, 0.0)
sunlineEKF.sunlineHMatrixYMeas(inputStates, numCSS, cssCos, sensorTresh, cssNormals, cssBias, obs, yMeas, numObs, measMat)
obsOut = []
yMeasOut = []
numObsOut = []
HOut = []
for i in range(8*6):
HOut.append(sunlineEKF.doubleArray_getitem(measMat, i))
for i in range(8):
yMeasOut.append(sunlineEKF.doubleArray_getitem(yMeas, i))
obsOut.append(sunlineEKF.doubleArray_getitem(obs, i))
numObsOut.append(sunlineEKF.intArray_getitem(numObs, 0))
#Fill in expected values for test
expectedH = np.zeros([8,6])
expectedY = np.zeros(8)
for j in range(3):
expectedH[j,0:3] = np.eye(3)[j,:]
expectedY[j] =np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j*3:(j+1)*3])
expectedObs = np.array([np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)),0.,0.,0.,0.,0.])
expectedNumObs = 3
HOut = np.array(HOut).reshape([8, 6])
errorNorm = np.zeros(4)
errorNorm[0] = np.linalg.norm(HOut - expectedH)
errorNorm[1] = np.linalg.norm(yMeasOut - expectedY)
errorNorm[2] = np.linalg.norm(obsOut - expectedObs)
errorNorm[3] = np.linalg.norm(numObsOut[0] - expectedNumObs)
for i in range(4):
if(errorNorm[i] > 1.0E-10):
testFailCount += 1
testMessages.append("H and yMeas update failure \n")
###################################################################################
## Test the Kalman Gain
###################################################################################
numObs = 3
h = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 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.]
covar = [1., 0., 0., 1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1., 0., 0., 1.,
1., 0., 0., 1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1., 0., 0., 1.]
noise= 0.01
Kalman = sunlineEKF.new_doubleArray(6 * 8)
for i in range(8 * 6):
sunlineEKF.doubleArray_setitem(Kalman, i, 0.)
sunlineEKF.sunlineKalmanGain(covar, h, noise, numObs, Kalman)
KalmanOut = []
for i in range(8 * 6):
KalmanOut.append(sunlineEKF.doubleArray_getitem(Kalman, i))
# Fill in expected values for test
Hmat = np.array(h).reshape([8,6])
Pk = np.array(covar).reshape([6,6])
R = noise*np.eye(3)
expectedK = np.dot(np.dot(Pk, Hmat[0:numObs,:].T), np.linalg.inv(np.dot(np.dot(Hmat[0:numObs,:], Pk), Hmat[0:numObs,:].T) + R[0:numObs,0:numObs]))
KalmanOut = np.array(KalmanOut)[0:6*numObs].reshape([6, 3])
errorNorm = np.linalg.norm(KalmanOut[:,0:numObs] - expectedK)
if (errorNorm > 1.0E-10):
print(errorNorm)
testFailCount += 1
testMessages.append("Kalman Gain update failure \n")
###################################################################################
## Test the EKF update
###################################################################################
KGain = [1.,2.,3., 0., 1., 2., 1., 0., 1., 0., 1., 0., 3., 0., 1., 0., 2., 0.]
for i in range(6*8-6*3):
KGain.append(0.)
inputStates = [2,1,0.75,0.1,0.4,0.05]
xbar = [0.1, 0.2, 0.01, 0.005, 0.009, 0.001]
numObs = 3
h = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 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.]
covar = [1., 0., 0., 1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1., 0., 0., 1.,
1., 0., 0., 1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1., 0., 0., 1.]
noise = 0.01
inputY = np.zeros(3)
for j in range(3):
inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j * 3:(j + 1) * 3])
inputY = inputY.tolist()
stateError = sunlineEKF.new_doubleArray(6)
covarMat = sunlineEKF.new_doubleArray(6*6)
inputs = sunlineEKF.new_doubleArray(6)
for i in range(6):
sunlineEKF.doubleArray_setitem(stateError, i, 0.)
sunlineEKF.doubleArray_setitem(inputs, i, inputStates[i])
for j in range(6):
sunlineEKF.doubleArray_setitem(covarMat,i+j,0.)
sunlineEKF.sunlineEKFUpdate(KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat)
stateOut = []
covarOut = []
errorOut = []
for i in range(6):
stateOut.append(sunlineEKF.doubleArray_getitem(inputs, i))
errorOut.append(sunlineEKF.doubleArray_getitem(stateError, i))
for j in range(36):
covarOut.append(sunlineEKF.doubleArray_getitem(covarMat, j))
# Fill in expected values for test
KK = np.array(KGain)[0:6*3].reshape([6,3])
expectedStates = np.array(inputStates) + np.dot(KK, np.array(inputY))
H = np.array(h).reshape([8,6])[0:3,:]
Pk = np.array(covar).reshape([6, 6])
R = noise * np.eye(3)
expectedP = np.dot(np.dot(np.eye(6) - np.dot(KK, H), Pk), np.transpose(np.eye(6) - np.dot(KK, H))) + np.dot(KK, np.dot(R,KK.T))
errorNorm = np.zeros(2)
errorNorm[0] = np.linalg.norm(np.array(stateOut) - expectedStates)
errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([6,6]))
for i in range(2):
if(errorNorm[i] > 1.0E-10):
testFailCount += 1
testMessages.append("EKF update failure \n")
###################################################################################
## Test the CKF update
###################################################################################
KGain = [1., 2., 3., 0., 1., 2., 1., 0., 1., 0., 1., 0., 3., 0., 1., 0., 2., 0.]
for i in range(6 * 8 - 6 * 3):
KGain.append(0.)
inputStates = [2, 1, 0.75, 0.1, 0.4, 0.05]
xbar = [0.1, 0.2, 0.01, 0.005, 0.009, 0.001]
numObs = 3
h = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 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.]
covar = [1., 0., 0., 1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1., 0., 0., 1.,
1., 0., 0., 1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1., 0., 0., 1.]
noise =0.01
inputY = np.zeros(3)
for j in range(3):
inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3],
np.array(cssNormals)[j * 3:(j + 1) * 3])
inputY = inputY.tolist()
stateError = sunlineEKF.new_doubleArray(6)
covarMat = sunlineEKF.new_doubleArray(6 * 6)
for i in range(6):
sunlineEKF.doubleArray_setitem(stateError, i, xbar[i])
for j in range(6):
sunlineEKF.doubleArray_setitem(covarMat, i + j, 0.)
sunlineEKF.sunlineCKFUpdate(xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat)
covarOut = []
errorOut = []
for i in range(6):
errorOut.append(sunlineEKF.doubleArray_getitem(stateError, i))
for j in range(36):
covarOut.append(sunlineEKF.doubleArray_getitem(covarMat, j))
# Fill in expected values for test
KK = np.array(KGain)[0:6 * 3].reshape([6, 3])
H = np.array(h).reshape([8, 6])[0:3, :]
expectedStateError = np.array(xbar) + np.dot(KK, (np.array(inputY) - np.dot(H, np.array(xbar))))
Pk = np.array(covar).reshape([6, 6])
expectedP = np.dot(np.dot(np.eye(6) - np.dot(KK, H), Pk), np.transpose(np.eye(6) - np.dot(KK, H))) + np.dot(KK,
np.dot(
R,
KK.T))
errorNorm = np.zeros(2)
errorNorm[0] = np.linalg.norm(np.array(errorOut) - expectedStateError)
errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([6, 6]))
for i in range(2):
if (errorNorm[i] > 1.0E-10):
testFailCount += 1
testMessages.append("CKF update failure \n")
# print out success message if no error were found
if testFailCount == 0:
print("PASSED: " + " EKF individual tests")
else:
print(testMessages)
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, ''.join(testMessages)]
####################################################################################
# Test for the time and update with static states (zero d_dot)
####################################################################################
def StatePropStatic():
# 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 = sunlineEKF.sunlineEKF()
module.ModelTag = "SunlineEKF"
# Add test module to runtime call list
unitTestSim.AddModelToTask(unitTaskName, module)
setupFilterData(module)
kfLog = module.logger(["covar", "state"], testProcessRate*10)
unitTestSim.AddModelToTask(unitTaskName, kfLog)
# connect messages
cssDataInMsg = messaging.CSSArraySensorMsg()
cssConfigInMsg = messaging.CSSConfigMsg()
module.cssDataInMsg.subscribeTo(cssDataInMsg)
module.cssConfigInMsg.subscribeTo(cssConfigInMsg)
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(macros.sec2nano(8000.0))
unitTestSim.ExecuteSimulation()
stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state)
for i in range(6):
if (abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0E-10):
testFailCount += 1
testMessages.append("State propagation failure \n")
# print out success message if no error were found
if testFailCount == 0:
print("PASSED: " + "EKF static state propagation")
else:
print(testMessages)
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, ''.join(testMessages)]
####################################################################################
# Test for the time and update with changing states (non-zero d_dot)
####################################################################################
def StatePropVariable(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
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 = sunlineEKF.sunlineEKF()
module.ModelTag = "SunlineEKF"
# Add test module to runtime call list
unitTestSim.AddModelToTask(unitTaskName, module)
setupFilterData(module)
InitialState = (np.array(module.state)+ +np.array([0.,0.,0.,0.0001,0.002, 0.001])).tolist()
Initialx = module.x
InitialCovar = module.covar
module.state = InitialState
kfLog = module.logger(["covar", "stateTransition", "state", "x"], testProcessRate)
unitTestSim.AddModelToTask(unitTaskName, kfLog)
# connect messages
cssDataInMsg = messaging.CSSArraySensorMsg()
cssConfigInMsg = messaging.CSSConfigMsg()
module.cssDataInMsg.subscribeTo(cssDataInMsg)
module.cssConfigInMsg.subscribeTo(cssConfigInMsg)
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(macros.sec2nano(1000.0))
unitTestSim.ExecuteSimulation()
covarLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.covar)
stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state)
stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x)
stmLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.stateTransition)
dt = 0.5
expectedStateArray = np.zeros([2001,7])
expectedStateArray[0,1:7] = np.array(InitialState)
for i in range(1,2001):
expectedStateArray[i,0] = dt*i*1E9
expectedStateArray[i,1:4] = expectedStateArray[i-1,1:4] + dt*(expectedStateArray[i-1,4:7] - (np.dot(expectedStateArray[i-1,4:7],expectedStateArray[i-1,1:4]))*expectedStateArray[i-1,1:4]/np.linalg.norm(expectedStateArray[i-1,1:4])**2.)
## Equations when removing the unobservable states from d_dot
expectedStateArray[i, 4:7] = expectedStateArray[i-1,4:7] - (np.dot(expectedStateArray[i-1,4:7],expectedStateArray[i-1,1:4]))*expectedStateArray[i-1,1:4]/np.linalg.norm(expectedStateArray[i-1,1:4])**2.
expDynMat = np.zeros([2001,6,6])
for i in range(0,2001):
expDynMat[i, 0:3, 0:3] = -(np.outer(expectedStateArray[i,1:4],expectedStateArray[i,4:7])/np.linalg.norm(expectedStateArray[i,1:4])**2. +
np.dot(expectedStateArray[i,4:7], expectedStateArray[i,1:4])*(np.linalg.norm(expectedStateArray[i,1:4])**2.*np.eye(3)- 2*np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4]))/np.linalg.norm(expectedStateArray[i,1:4])**4.)
expDynMat[i, 0:3, 3:6] = np.eye(3) - np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4])/np.linalg.norm(expectedStateArray[i,1:4])**2
## Equations when removing the unobservable states from d_dot
expDynMat[i, 3:6, 0:3] = -1/dt*(np.outer(expectedStateArray[i,1:4],expectedStateArray[i,4:7])/np.linalg.norm(expectedStateArray[i,1:4])**2. +
np.dot(expectedStateArray[i,4:7], expectedStateArray[i,1:4])*(np.linalg.norm(expectedStateArray[i,1:4])**2.*np.eye(3)- 2*np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4]))/np.linalg.norm(expectedStateArray[i,1:4])**4.)
expDynMat[i, 3:6, 3:6] = -1/dt*(np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4])/np.linalg.norm(expectedStateArray[i,1:4])**2)
expectedSTM = np.zeros([2001,6,6])
expectedSTM[0,:,:] = np.eye(6)
for i in range(1,2001):
expectedSTM[i,:,:] = dt * np.dot(expDynMat[i-1,:,:], np.eye(6)) + np.eye(6)
expectedXBar = np.zeros([2001,7])
expectedXBar[0,1:7] = np.array(Initialx)
for i in range(1,2001):
expectedXBar[i,0] = dt*i*1E9
expectedXBar[i, 1:7] = np.dot(expectedSTM[i, :, :], expectedXBar[i - 1, 1:7])
expectedCovar = np.zeros([2001,37])
expectedCovar[0,1:37] = np.array(InitialCovar)
Gamma = np.zeros([6, 3])
Gamma[0:3, 0:3] = dt ** 2. / 2. * np.eye(3)
Gamma[3:6, 0:3] = dt * np.eye(3)
ProcNoiseCovar = np.dot(Gamma, np.dot(module.qProcVal*np.eye(3),Gamma.T))
for i in range(1,2001):
expectedCovar[i,0] = dt*i*1E9
expectedCovar[i,1:37] = (np.dot(expectedSTM[i,:,:], np.dot(np.reshape(expectedCovar[i-1,1:37],[6,6]), np.transpose(expectedSTM[i,:,:])))+ ProcNoiseCovar).flatten()
FilterPlots.StatesVsExpected(stateLog, expectedStateArray, show_plots)
FilterPlots.StatesPlotCompare(stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots)
for j in range(1,2001):
for i in range(6):
if (abs(stateLog[j, i + 1] - expectedStateArray[j, i + 1]) > 1.0E-4):
testFailCount += 1
testMessages.append("General state propagation failure: State Prop \n")
if (abs(stateErrorLog[j, i + 1] - expectedXBar[j, i + 1]) > 1.0E-4):
testFailCount += 1
testMessages.append("General state propagation failure: State Error Prop \n")
for i in range(36):
if (abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) > 1.0E-4):
abs(covarLog[j, i + 1] - expectedCovar[j, i + 1])
testFailCount += 1
testMessages.append("General state propagation failure: Covariance Prop \n")
if (abs(stmLog[j, i + 1] - expectedSTM[j,:].flatten()[i]) > 1.0E-4):
testFailCount += 1
testMessages.append("General state propagation failure: STM Prop \n")
# print out success message if no error were found
if testFailCount == 0:
print("PASSED: " + "EKF general state propagation")
else:
print(testMessages)
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, ''.join(testMessages)]
####################################################################################
# Test for the full filter with time and measurement update
####################################################################################
def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess):
# 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 = sunlineEKF.sunlineEKF()
module.ModelTag = "SunlineEKF"
# Add test module to runtime call list
unitTestSim.AddModelToTask(unitTaskName, module)
setupFilterData(module)
# Set up some test parameters
cssConstelation = messaging.CSSConfigMsgPayload()
CSSOrientationList = [
[0.70710678118654746, -0.5, 0.5],
[0.70710678118654746, -0.5, -0.5],
[0.70710678118654746, 0.5, -0.5],
[0.70710678118654746, 0.5, 0.5],
[-0.70710678118654746, 0, 0.70710678118654757],
[-0.70710678118654746, 0.70710678118654757, 0.0],
[-0.70710678118654746, 0, -0.70710678118654757],
[-0.70710678118654746, -0.70710678118654757, 0.0],
]
CSSBias = [1 for i in range(len(CSSOrientationList))]
totalCSSList = []
# Initializing a 2D double array is hard with SWIG. That's why there is this
# layer between the above list and the actual C variables.
i = 0
for CSSHat in CSSOrientationList:
newCSS = messaging.CSSUnitConfigMsgPayload()
newCSS.CBias = CSSBias[i]
newCSS.nHat_B = CSSHat
totalCSSList.append(newCSS)
i = i+1
cssConstelation.nCSS = len(CSSOrientationList)
cssConstelation.cssVals = totalCSSList
inputData = messaging.CSSArraySensorMsgPayload()
cssConstInMsg = messaging.CSSConfigMsg().write(cssConstelation)
cssDataInMsg = messaging.CSSArraySensorMsg()
# connect messages
module.cssDataInMsg.subscribeTo(cssDataInMsg)
module.cssConfigInMsg.subscribeTo(cssConstInMsg)
stateTarget1 = testVector1
stateTarget1 += [0.0, 0.0, 0.0]
module.state = stateGuess
module.x = (np.array(stateTarget1) - np.array(stateGuess)).tolist()
kfLog = module.logger("x", testProcessRate)
dataLog = module.filtDataOutMsg.recorder()
unitTestSim.AddModelToTask(unitTaskName, dataLog)
unitTestSim.AddModelToTask(unitTaskName, kfLog)
unitTestSim.InitializeSimulation()
for i in range(SimHalfLength):
if i > 20:
dotList = []
for element in CSSOrientationList:
if AddMeasNoise:
dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) + np.random.normal(0., module.qObsVal)
else:
dotProd = np.dot(np.array(element), np.array(testVector1)[0:3])
dotList.append(dotProd)
inputData.CosValue = dotList
cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos)
unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5))
unitTestSim.ExecuteSimulation()
stateLog = addTimeColumn(dataLog.times(), dataLog.state)
covarLog = addTimeColumn(dataLog.times(), dataLog.covar)
for i in range(6):
if (abs(covarLog[-1, i * 6 + 1 + i] - covarLog[0, i * 6 + 1 + i] / 100.) > 1E-2):
testFailCount += 1
testMessages.append("Covariance update failure")
if (abs(stateLog[-1, i + 1] - stateTarget1[i]) > 1.0E-2):
testFailCount += 1
testMessages.append("State update failure")
stateTarget2 = testVector2
stateTarget2 = stateTarget2+[0.,0.,0.]
inputData = messaging.CSSArraySensorMsgPayload()
for i in range(SimHalfLength):
if i > 20:
dotList = []
for element in CSSOrientationList:
if AddMeasNoise:
dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) + np.random.normal(0., module.qObsVal)
else:
dotProd = np.dot(np.array(element), np.array(testVector2)[0:3])
dotList.append(dotProd)
inputData.CosValue = dotList
cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos)
unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength+1) * 0.5))
unitTestSim.ExecuteSimulation()
stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x)
stateLog = addTimeColumn(dataLog.times(), dataLog.state)
postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes)
covarLog = addTimeColumn(dataLog.times(), dataLog.covar)
for i in range(6):
if (abs(covarLog[-1, i * 6 + 1 + i] - covarLog[0, i * 6 + 1 + i] / 100.) > 1E-2):
testFailCount += 1
testMessages.append("Covariance update failure")
if (abs(stateLog[-1, i + 1] - stateTarget2[i]) > 1.0E-2):
testFailCount += 1
testMessages.append("State update failure")
target1 = np.array(testVector1)
target2 = np.array(testVector2+[0.,0.,0.])
FilterPlots.StateErrorCovarPlot(stateErrorLog, covarLog, show_plots)
FilterPlots.StatesVsTargets(target1, target2, stateLog, show_plots)
FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, show_plots)
# print out success message if no error were found
if testFailCount == 0:
print("PASSED: " + "EKF full test")
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, ''.join(testMessages)]
if __name__ == "__main__":
# test_all_sunline_ekf(True, 200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0])
# StatePropVariable(True)
# StatePropStatic()
StateUpdateSunLine(True, 200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0])