├── Core ├── __init__.py ├── SensorModel.py ├── Overview.py ├── Servo.py ├── Systems.py ├── AirDataCalibration.py ├── OpenData.py ├── KinematicTransforms.py ├── Environment.py ├── AirData.py ├── GenExcite.py └── Loader.py ├── Documentation └── Simulation Equation Summary.docx ├── setup.py ├── README.md ├── LICENSE.md ├── Tests ├── TestRobustStab_SP_8_9.m ├── MultisinePolarPlotExample.py ├── TestChirp.py ├── WindowFits.py ├── Turbulence.py ├── TestRobustStab_SP_8_9.py ├── TestOMS.py ├── TestFreqRespEst.py ├── TestSchroeder.py ├── TestFreqRespEstUS25e.py └── TestMultisineSpacing.py ├── .gitignore └── Examples ├── DiskMargin.py ├── HuginnFLTXX.py ├── HuginnLanding.py ├── HuginnBend.py ├── HuginnAirCal.py ├── HuginnAirCal2.py ├── HuginnRtsm.py ├── ThorRtsm.py ├── HuginnAeroPID.py └── HuginnRtsm_Disturb.py /Core/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Documentation/Simulation Equation Summary.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UASLab/OpenFlightAnalysis/HEAD/Documentation/Simulation Equation Summary.docx -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | from distutils.core import setup 11 | 12 | setup( 13 | name = 'OpenFlightAnalysis', 14 | author = 'Chris Regan', 15 | packages = ['Test', 'Core'], 16 | license = 'LICENSE.md', 17 | long_description = open('README.md').read(), 18 | ) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OpenFlightAnalysis 2 | Open Flight Analysis (oFA) 3 | 4 | www.uav.aem.umn.edu 5 | 6 | oFA is a companion to OpenFlightSim for simulation and RAPTRS for flight systems. 7 | 8 | https://github.com/UASLab/OpenFlightSim 9 | 10 | https://github.com/bolderflight/RAPTRS 11 | 12 | oFA is intended as a tool to analyze flight vehicle data. Both time-domain and frequency-domain based analysis are envisioned. Flight (and oFS) data are loaded and formated into an intermediate data structure known as oData (Open Data). A complete flight data record is segmented into individual segments and stored into an easily manipulated format. This allows for flight analysis that easily spans multiple flights and flight configurations. Flight/Simulation data logged from oFS and RAPTRS can be automatically loaded and segmented. 13 | 14 | Specialty tools for conducting In-flight sensor calibration, Aerodynamic Parameter Identification (Aero-PID), and in-flight stability analysis (sometime refered to as Real-time Stability Margin analysis - RTSM) are to be incorporated. 15 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) Copyright 2019 Regents of the University of Minnesota. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Core/SensorModel.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | 12 | def SensorErrorModel(xMeas, param): 13 | ''' Compute the "True" Measure based on the error model parameters. 14 | % 15 | %Inputs: 16 | % xMeas - measures 17 | % param - error model parameters [errorType = 'None'] 18 | % 19 | %Outputs: 20 | % xTrue - true 21 | % 22 | %Notes: 23 | % There are no internal unit conversions. 24 | % Generally, Flight data analysis uses the "-" version of the models, simulation would use "+" 25 | ''' 26 | 27 | ## Compute the True Measure 28 | if param['errorType'].lower() == 'none': 29 | xTrue = xMeas 30 | elif param['errorType'].lower() == 'bias+': # Bias 31 | xTrue = xMeas + param['bias'] 32 | elif param['errorType'].lower() == 'scalebias+': # Scale and Bias 33 | xTrue = param['K'] * xMeas + param['bias'] 34 | elif param['errorType'].lower() == 'quad+': # Quad, Scale, Bias 35 | xTrue = param['quad'] * xMeas**2 + param['K'] * xMeas + param['bias'] 36 | elif param['errorType'].lower() == 'sqrt+': # Sqrt, Scale, and Bias 37 | xTrue = param['quad'] * np.sqrt(xMeas) + param['K'] * xMeas + param['bias'] 38 | elif param['errorType'].lower() == 'inv+': # Inv, Scale, and Bias 39 | xTrue = param['inv'] / xMeas + param['K'] * xMeas + param['bias'] 40 | else: 41 | print('Unkown Option') 42 | 43 | 44 | return xTrue 45 | -------------------------------------------------------------------------------- /Tests/TestRobustStab_SP_8_9.m: -------------------------------------------------------------------------------- 1 | % Example 8.9 Produces fig. 8.11 2 | % 3 | % Copyright 1996-2003 Sigurd Skogestad & Ian Postlethwaite 4 | % $Id: Expl8_9.m,v 1.1 2004/01/21 15:21:24 vidaral Exp $ 5 | % clear all; close all; 6 | omega = logspace(-3,2,101); 7 | 8 | tau = 75; 9 | G = tf([0 1],[tau 1])*[-87.8 1.4;-108.2 -1.4]; % Plant 10 | K = tf([tau 1],[1 1e-6])*[-0.0015 0; 0 -0.075]; % Controller 11 | wI = 0.2*tf([5 1],[0.5 1]); % Input uncertainty 12 | 13 | Li = K * G; 14 | Si = inv(eye(2) + Li); 15 | Ti = Si * Li; 16 | 17 | Tif = frd(Ti, omega); 18 | Wif = frd(wI, omega); 19 | 20 | svTi = sigma(Ti, omega); 21 | 22 | blk=[1 1;1 1]; % all 1-by-1 complex blocks 23 | [muTi_bnds, muTi_info] = mussv(Tif, blk); 24 | 25 | figure(1); % 'Figure 8.11' 26 | loglog(omega, squeeze(1/frdata(Wif)), 'r--'); hold on; grid on; 27 | loglog(omega, squeeze(min(frdata(muTi_bnds))), 'b-'); 28 | loglog(omega, svTi(1,:), 'g-'); 29 | loglog(omega, svTi(2,:), 'g--'); 30 | legend({'1/|W_i|', '\mu_{\Delta_I}(T_i)', 'max \sigma(T_i)', 'min \sigma(T_i)'}); 31 | 32 | hold off 33 | axis auto 34 | 35 | %% 36 | M = Tif * Wif; 37 | svM = sigma(M, omega); 38 | svM_max = max(svM); 39 | 40 | [muMbnds, muMinfo] = mussv(M, blk); 41 | 42 | muM = squeeze(min(frdata(muMbnds))); 43 | km = 1./muM; 44 | 45 | figure(2); 46 | semilogx(omega, mag2db(1./svM_max), 'g-'); hold on; grid on; 47 | semilogx(omega, mag2db(km), 'b-'); 48 | semilogx(omega, mag2db(1) * ones(size(omega)), 'r-'); 49 | semilogx(omega, mag2db(1/0.4) * ones(size(omega)), 'r--'); 50 | % legend({'max \sigma(M)', '\mu_{\Delta_I}(M)', 'Crit = 0', 'Crit = 0.4'}); 51 | legend({'max \sigma(M)', '\mu_{\Delta_I}(M)'}); 52 | xlabel('Frequency [rad/sec]') 53 | ylabel('Margin [dB]') 54 | 55 | 56 | hold off 57 | axis auto -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | .DS_Store 107 | *.stats 108 | -------------------------------------------------------------------------------- /Examples/DiskMargin.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import matplotlib.patches 5 | # Hack to allow loading the Core package 6 | if __name__ == "__main__" and __package__ is None: 7 | from sys import path, argv 8 | from os.path import dirname, abspath, join 9 | 10 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 11 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 12 | 13 | del path, argv, dirname, abspath, join 14 | 15 | import FreqTrans 16 | 17 | #%% 18 | pCrit = -1+0j 19 | T = np.array([-0.5 - 0.5j]) 20 | TUnc = np.array([0.5 + 0.25j]) 21 | 22 | rCritNom, rCritUnc, rCrit, pCont = FreqTrans.DistCritEllipse(T, TUnc, pCrit = pCrit) 23 | 24 | #rCritNomCirc, rCritUncCirc, rCritCirc = FreqTrans.DistCritCirc(T, TUnc, pCrit = pCrit, typeNorm = 'RMS') 25 | rCirc = np.sqrt(0.5) * np.abs(TUnc) # RMS 26 | #rCirc = np.max([TUnc.real, TUnc.imag]) # Max 27 | #rCirc = np.mean([TUnc.real, TUnc.imag]) # Mean 28 | #rCirc = np.abs(TUnc) # RSS 29 | 30 | TUncCirc = np.array([rCirc+1j*rCirc]) 31 | rCritNomCirc, rCritUncCirc, rCritCirc, pContCirc = FreqTrans.DistCritEllipse(T, TUncCirc, pCrit = pCrit) 32 | 33 | 34 | 35 | #% 36 | fig, ax = plt.subplots(nrows=1, ncols=1) 37 | 38 | ax.plot(T.real, T.imag, 'b*-') 39 | ax.plot([pCrit.real, T.real], [pCrit.imag, T.imag], 'r*:') 40 | 41 | ellipse = matplotlib.patches.Ellipse(xy = [T.real, T.imag], width=2*TUnc.real, height=2*TUnc.imag, color='b', alpha = 0.5) 42 | ax.add_patch(ellipse) 43 | ax.plot([pCrit.real, pCont.real], [pCrit.imag, pCont.imag], 'b*--') 44 | 45 | 46 | circ = matplotlib.patches.Ellipse(xy = [T.real, T.imag], width=2*TUncCirc.real, height=2*TUncCirc.imag, color='g', alpha = 0.5) 47 | ax.add_patch(circ) 48 | ax.plot([pCrit.real, pContCirc.real], [pCrit.imag, pContCirc.imag], 'g*--') 49 | 50 | 51 | ax.axis('equal') 52 | fig.suptitle(['Nom: ' + str(rCritNom[0]) + ' Ellipse: ' + str(rCrit[0]) + ', Circle: ' + str(rCritCirc[0])]) 53 | 54 | 55 | -------------------------------------------------------------------------------- /Tests/MultisinePolarPlotExample.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Mar 7 12:32:14 2021 4 | 5 | @author: Chris 6 | """ 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | # Hack to allow loading the Core package 12 | if __name__ == "__main__" and __package__ is None: 13 | from sys import path, argv 14 | from os.path import dirname, abspath, join 15 | 16 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 17 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 18 | 19 | del path, argv, dirname, abspath, join 20 | 21 | from Core import GenExcite 22 | 23 | 24 | # Constants 25 | pi = np.pi 26 | 27 | hz2rps = 2*pi 28 | rps2hz = 1/hz2rps 29 | 30 | rad2deg = 180/pi 31 | deg2rad = 1/rad2deg 32 | 33 | 34 | #%% 35 | numChan = 1 36 | freqRate_hz = 1000; 37 | timeDur_s = 1.0 38 | numCycles = 1 39 | 40 | freqMinDes_rps = (1/timeDur_s) * hz2rps * np.ones(numChan) 41 | freqMaxDes_rps = 10 * hz2rps * np.ones(numChan) 42 | freqStepDes_rps = 1 * hz2rps 43 | methodSW = 'zip' # "zippered" component distribution 44 | 45 | ## Generate MultiSine Frequencies 46 | freqExc_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW) 47 | timeDur_s = time_s[-1] - time_s[0] 48 | 49 | ## Generate Schroeder MultiSine Signal 50 | ampElem_nd = np.ones_like(freqExc_rps) ## Approximate relative signal amplitude, create flat 51 | # ampElem_nd = np.sqrt(2 / freqExc_rps**3) # Pwr/J = 0.5 * A**2 * freq_rps**3 52 | sigList, phaseElem_rad, sigElem = GenExcite.MultiSine(freqExc_rps, ampElem_nd, sigIndx, time_s, costType = 'Schroeder', phaseInit_rad = 0, boundPhase = True, initZero = True, normalize = 'rms'); 53 | sigPeakFactor = GenExcite.PeakFactor(sigList) 54 | 55 | sigElem *= 0.3 56 | 57 | numPts = time_s.shape[-1] 58 | angle = np.linspace(0, freqMinDes_rps[0], numPts) 59 | 60 | xDir = np.cos(angle) 61 | yDir = np.sin(angle) 62 | 63 | xCent = 0.0 64 | yCent = 0.0 65 | R = 1 66 | 67 | xBase = xCent + R * xDir 68 | yBase = yCent + R * yDir 69 | 70 | # Plot 71 | plt.figure() 72 | plt.plot(time_s, sigElem.T) 73 | 74 | xWaveList = [] 75 | yWaveList = [] 76 | for i, d in enumerate(sigElem): 77 | xWave = xBase + d * xDir 78 | yWave = yBase + d * yDir 79 | 80 | xWaveList.append(xWave) 81 | yWaveList.append(yWave) 82 | 83 | plt.figure() 84 | plt.plot(xBase, yBase, color = 'k', linestyle = ':') 85 | plt.plot(np.array(xWaveList).T, np.array(yWaveList).T, color = 'gray') 86 | -------------------------------------------------------------------------------- /Examples/HuginnFLTXX.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) FLT XX 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import os 15 | import numpy as np 16 | import matplotlib.pyplot as plt 17 | 18 | # Hack to allow loading the Core package 19 | if __name__ == "__main__" and __package__ is None: 20 | from sys import path, argv 21 | from os.path import dirname, abspath, join 22 | 23 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 24 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 25 | 26 | del path, argv, dirname, abspath, join 27 | 28 | from Core import Loader 29 | from Core import OpenData 30 | 31 | 32 | # Constants 33 | hz2rps = 2 * np.pi 34 | rps2hz = 1 / hz2rps 35 | 36 | pathBase = os.path.join('/home', 'rega0051', 'FlightArchive') 37 | ac = 'Huginn' 38 | flt = 'FLT03' 39 | 40 | fileLog = os.path.join(pathBase, ac, ac + flt, ac + flt + '.h5') 41 | fileTestDef = os.path.join(pathBase, ac, ac + flt, ac.lower() + '_def.json') 42 | fileSysConfig = os.path.join(pathBase, ac, ac + flt, ac.lower() + '.json') 43 | 44 | 45 | #%% 46 | # Read in raw h5 data into dictionary and data 47 | oData, h5Data = Loader.Log_RAPTRS(fileLog, fileSysConfig) 48 | 49 | # Plot Overview of flight 50 | #oData = OpenData.Segment(oData, ('time_s', [950, 970])) 51 | OpenData.PlotOverview(oData) 52 | 53 | 54 | #%% Find Excitation Times 55 | excList = OpenData.FindExcite(oData) 56 | segList = [] 57 | print('\n\nFlight Excitation Times:\n') 58 | for iExc in range(0, len(excList)): 59 | print('Excitiation: ', excList[iExc][0], ', Time: [', excList[iExc][1][0], ',', excList[iExc][1][1], ']') 60 | 61 | segList.append( ('time_us', excList[iExc][1])) 62 | 63 | oDataExc = OpenData.Segment(oData, segList) 64 | #OpenData.PlotOverview(oDataExc[0]) 65 | 66 | 67 | #%% Save _init.json file 68 | # Open init json file 69 | fltDef = {} 70 | #fltDef = JsonRead(fileTestDef) 71 | 72 | # Open flight json file 73 | fltConfig = Loader.JsonRead(fileSysConfig) 74 | testPointList = fltConfig['Mission-Manager']['Test-Points'] 75 | 76 | fltDef['Test-Points'] = OpenData.TestPointOut(excList, testPointList) 77 | 78 | if True: 79 | Loader.JsonWrite(fileTestDef, fltDef) 80 | print('Init File Updated:\n', fileTestDef) 81 | else: 82 | import json 83 | json.dumps(fltDef, indent = 4, ensure_ascii=False) 84 | print('\nInit file NOT updated\n\n') 85 | 86 | -------------------------------------------------------------------------------- /Tests/TestChirp.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Exampe script for generating sin sweep type excitations. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # Hack to allow loading the Core package 16 | if __name__ == "__main__" and __package__ is None: 17 | from sys import path, argv 18 | from os.path import dirname, abspath, join 19 | 20 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 21 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 22 | 23 | del path, argv, dirname, abspath, join 24 | 25 | from Core import GenExcite 26 | from Core import FreqTrans 27 | 28 | 29 | # Constants 30 | hz2rps = 2*np.pi 31 | rps2hz = 1/hz2rps 32 | 33 | #%% Define the frequency selection and distribution of the frequencies into the signals 34 | freqRate_hz = 50; 35 | freqInit_rps = 0.1 * hz2rps 36 | freqFinal_rps = 10 * hz2rps 37 | timeDur_s = 10.0 38 | ampInit = 1.0 39 | ampFinal = 1.0 40 | 41 | time_s = np.linspace(0, timeDur_s, int(timeDur_s * freqRate_hz) + 1) 42 | sig, ampChirp, freqChirp_rps = GenExcite.Chirp(freqInit_rps, freqFinal_rps, time_s, ampInit, ampFinal, freqType = 'linear', ampType = 'linear', initZero = 1) 43 | 44 | ## Results 45 | plt.figure() 46 | plt.subplot(3,1,1) 47 | plt.plot(time_s, sig); plt.grid() 48 | plt.ylabel('Signal (nd)'); 49 | plt.subplot(3,1,2) 50 | plt.plot(time_s, ampChirp); plt.grid() 51 | plt.ylabel('ampitude (nd)'); 52 | plt.subplot(3,1,3) 53 | plt.plot(time_s, freqChirp_rps * rps2hz); plt.grid() 54 | plt.xlabel('Time (s)'); plt.ylabel('Frequency (Hz)') 55 | plt.show() 56 | 57 | 58 | #%% Plot the Excitation Spectrum 59 | ## Compute Spectrum of each channel 60 | optSpect = FreqTrans.OptSpect(freqRate_rps = freqRate_hz * hz2rps) 61 | 62 | freq_hz, sigDft, Psd_mag = FreqTrans.Spectrum(sig, optSpect) 63 | Psd_dB = 20*np.log10(Psd_mag) 64 | 65 | ## Plot Spectrum 66 | plt.figure() 67 | plt.plot(freq_hz[0], Psd_dB[0]) 68 | plt.xlabel('frequency (Hz)'); 69 | plt.ylabel('Spectrum (dB)'); 70 | plt.grid() 71 | plt.show() 72 | 73 | 74 | #%% Create the output for JSON config 75 | timeFinal_s = time_s[-1] 76 | timeStart_s = time_s[0] 77 | 78 | jsonChirp = {} 79 | jsonChirp['Type'] = 'LinearChirp' 80 | jsonChirp['Duration'] = timeDur_s 81 | jsonChirp['ampitude'] = [ampInit, ampFinal] 82 | jsonChirp['Frequency'] = [freqInit_rps, freqFinal_rps] 83 | 84 | import json 85 | print(json.dumps(['Chirp', jsonChirp], separators=(', ', ': '))) 86 | 87 | -------------------------------------------------------------------------------- /Core/Overview.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Louis Mueller 5 | University of Minnesota UAV Lab 6 | 7 | Description: 8 | Overview is used for general analysis of .h5 flight data. General flight overview 9 | plots and excitation numbers and timestamps will be displayed. The [--save] option 10 | will update a file in the given flight folder named _init.json with all 11 | excitation details and times given in the .json given in the same flight folder. 12 | Timestamps and h5 data can then be used using analysis.py. 13 | 14 | General Steps: 15 | 1. Load .h5 and .json from flightFolder 16 | 2. Plot general figures 17 | 3. Save to _init.json 18 | 19 | Example: 20 | python Overview.py [flightFolder] [--save] 21 | python Overview.py ThorFLT123 --save 22 | 23 | Future Improvements: 24 | - Overview of sim csv 25 | - Save figures 26 | """ 27 | 28 | # Import Libraries 29 | import os 30 | import json 31 | import argparse 32 | 33 | import Loader 34 | import OpenData 35 | 36 | #%% Parse and Load 37 | # Include arguments for load 38 | parser = argparse.ArgumentParser(description = "Load h5 file and load overview plots") 39 | parser.add_argument("path") 40 | parser.add_argument("fileLogName") 41 | parser.add_argument("fileDefName") 42 | parser.add_argument("fileConfigName") 43 | parser.add_argument("--save", action = "store_true") 44 | args = parser.parse_args() 45 | 46 | pathData = str(args.path) 47 | fileLogName = str(args.fileLogName) 48 | fileDefName = str(args.fileDefName) 49 | fileConfigName = str(args.fileConfigName) 50 | 51 | # Locate files 52 | fileLog = os.path.join(pathData, fileLogName) 53 | fileTestDef = os.path.join(pathData, fileDefName) 54 | fileSysConfig = os.path.join(pathData, fileConfigName) 55 | 56 | 57 | #%% 58 | # Read in raw h5 data into dictionary and data 59 | oData, h5Data = Loader.Log_RAPTRS(fileLog) 60 | 61 | # Plot Overview of flight 62 | OpenData.PlotOverview(oData) 63 | 64 | #%% Find Excitation Times 65 | exciteID, exciteIndex = OpenData.FindExcite(oData) 66 | 67 | print('\n\nFlight Excitation Times:\n') 68 | for i in range(0, len(exciteID)): 69 | if i%2 == 0: 70 | print('Excitiation', exciteID[i], 'Time: (', exciteIndex[i], ',', exciteIndex[i + 1], ')') 71 | 72 | #%% Save _init.json file 73 | # Open init json file 74 | json_init = {} 75 | #json_init = JsonRead(fileTestDef) 76 | 77 | # Open flight json file 78 | flightjson = Loader.JsonRead(fileSysConfig) 79 | 80 | json_init['Test-Points'] = OpenData.TestPointOut(exciteID, exciteIndex, flightjson) 81 | 82 | if args.save: 83 | Loader.JsonWrite(fileTestDef, json_init) 84 | print('Init File Updated:\n', fileTestDef) 85 | else: 86 | json.dumps(json_init, indent = 4, ensure_ascii=False) 87 | print('\nInit file NOT updated\n\n') 88 | -------------------------------------------------------------------------------- /Tests/WindowFits.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | #%% 4 | import numpy as np 5 | from scipy import signal 6 | from scipy import fft 7 | import matplotlib.pyplot as plt 8 | import control 9 | 10 | # Hack to allow loading the Core package 11 | if __name__ == "__main__" and __package__ is None: 12 | from sys import path, argv 13 | from os.path import dirname, abspath, join 14 | 15 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 16 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 17 | 18 | del path, argv, dirname, abspath, join 19 | 20 | from Core import FreqTrans 21 | 22 | # Constants 23 | pi = np.pi 24 | 25 | mag2db = control.mag2db 26 | db2mag = control.db2mag 27 | 28 | #%% 29 | from scipy.optimize import curve_fit 30 | 31 | def func(b, a, s): 32 | W = 10**a * b**s 33 | return W 34 | 35 | lenX = 20 36 | N = 2**12 37 | Nhalf = int(N/2**1) 38 | 39 | winTypeList = ['Boxcar', 'Bartlett', 'Hann'] 40 | winLabelList = ['Dirichlet', 'Bartlett', 'Hann'] 41 | colorList = ['b', 'g', 'r'] 42 | # winTypeList = ['Boxcar'] 43 | 44 | # fig1 = plt.figure() 45 | # ax1 = fig1.subplots(1,1) 46 | 47 | fig2 = plt.figure() 48 | ax2 = fig2.subplots(1,1) 49 | 50 | for i, winType in enumerate(winTypeList): 51 | winLabel = winLabelList[i] 52 | win = signal.get_window(winType.lower(), lenX) 53 | 54 | # ax1.plot(win, label = str(winType)) 55 | 56 | freq = np.linspace(0, 1, N) 57 | bins = freq * lenX 58 | 59 | A = fft.fft(win, N) 60 | M_mag = np.abs(A) / np.nanmax(np.abs(A)) 61 | M_dB = mag2db(M_mag) 62 | 63 | ax2.plot(bins[:Nhalf], M_dB[:Nhalf], linestyle = '-', color = colorList[i], label = str(winLabel)) 64 | 65 | iPeak, _ = signal.find_peaks(M_dB) 66 | iPeak = iPeak[bins[iPeak] < lenX/2] 67 | binPeak = bins[iPeak] 68 | MPeak_mag = M_mag[iPeak] 69 | MPeak_dB = M_dB[iPeak] 70 | 71 | color = ax2.get_lines()[-1].get_color() 72 | # ax2.plot(binPeak, MPeak_dB, '.', color = color, linestyle = 'None', label = str(winType) + ' Peaks') 73 | 74 | popt, pcov = curve_fit(func, binPeak, MPeak_mag) 75 | 76 | Mfit_mag = func(bins, *popt) 77 | Mfit_dB = mag2db(Mfit_mag) 78 | 79 | iSide1 = np.argwhere(bins > binPeak[0])[0][0]-20 80 | iStart = np.argwhere(Mfit_dB[:iSide1] < M_dB[:iSide1])[-1][0] 81 | ax2.plot(bins[iStart:], Mfit_dB[iStart:], color = color, linestyle = ':', label = str(winLabel) + ' Approx') 82 | 83 | print(str(winLabel) + 'binMin: ' + str(bins[iStart]) + ' a: ' + str(popt[0]) + ' s: ' + str(popt[1])) 84 | 85 | 86 | # ax1.set_ylabel("Amplitude") 87 | # ax1.set_xlabel("Sample") 88 | # ax1.grid(True) 89 | # ax1.set_ylim([0, 1.1]) 90 | # ax1.legend() 91 | 92 | ax2.set_xlim([0, 10]) 93 | ax2.set_ylim([-80, 10]) 94 | ax2.grid(True) 95 | ax2.set_ylabel("Normalized Power Magnitude [dB]") 96 | ax2.set_xlabel("Normalized Bin") 97 | ax2.legend(loc = 'upper right', framealpha = 1) 98 | 99 | fig2.set_size_inches([6.4,3.6]) 100 | fig2.tight_layout() 101 | 102 | if False: 103 | FreqTrans.PrintPrettyFig(fig2, 'WindowFunc.pgf') 104 | -------------------------------------------------------------------------------- /Tests/Turbulence.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri Jul 26 11:47:02 2019 4 | 5 | @author: rega0051 6 | """ 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | # Hack to allow loading the Core package 11 | if __name__ == "__main__" and __package__ is None: 12 | from sys import path, argv 13 | from os.path import dirname, abspath, join 14 | 15 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 16 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 17 | 18 | del path, argv, dirname, abspath, join 19 | 20 | import Environment 21 | 22 | hz2rps = 2 * np.pi 23 | rps2hz = 1/hz2rps 24 | 25 | #%% 26 | V_fps = 75 27 | h_ft = 300 28 | b_ft = 5 29 | level = 'Light' 30 | 31 | # Spatial Frequency 32 | omega = np.logspace(-4, 1, 300) 33 | 34 | sigma = Environment.TurbIntensity(h_ft, level = level) 35 | L_ft = Environment.TurbLengthScale(h_ft) 36 | 37 | # Linear Velocity 38 | Puvw_Dryden = Environment.TurbSpectDryden(sigma, L_ft, omega) 39 | Puvw_VonKarman = Environment.TurbSpectVonKarman(sigma, L_ft, omega) 40 | 41 | fig, ax = plt.subplots(nrows=3, sharex='all', sharey='all') 42 | ax[0].loglog(omega*rps2hz, Puvw_Dryden[0], label = "Dryden - Level: " + level) 43 | ax[0].loglog(omega*rps2hz, Puvw_VonKarman[0], label = "VonKarman - Level: " + level) 44 | ax[0].set_ylabel("Disturbance u"); ax[0].legend(); ax[0].grid(True) 45 | 46 | ax[1].loglog(omega*rps2hz, Puvw_Dryden[1], label = "Dryden - Level: " + level) 47 | ax[1].loglog(omega*rps2hz, Puvw_VonKarman[1], label = "VonKarman - Level: " + level) 48 | ax[1].set_ylabel("Disturbance v"); ax[1].legend(); ax[1].grid(True) 49 | 50 | ax[2].loglog(omega*rps2hz, Puvw_Dryden[2], label = "Dryden - Level: " + level) 51 | ax[2].loglog(omega*rps2hz, Puvw_VonKarman[2], label = "VonKarman - Level: " + level) 52 | ax[2].set_xlabel("Frequency (cycle/ft)"); ax[2].set_ylabel("Disturbance w"); ax[2].legend(); ax[2].grid(True) 53 | #ax[2].set_xlim([0.1, 50]) 54 | fig.suptitle("Turbulence Spectrum") 55 | 56 | 57 | #%% Rotation Rate 58 | freq_rps = omega * V_fps 59 | freq_hz = freq_rps / hz2rps 60 | 61 | Ppqr_Dryden = Environment.TurbSpectRate(Puvw_Dryden / V_fps, sigma, L_ft, freq_rps, V_fps, b_ft) 62 | Ppqr_VonKarman = Environment.TurbSpectRate(Puvw_VonKarman / V_fps, sigma, L_ft, freq_rps, V_fps, b_ft) 63 | 64 | fig, ax = plt.subplots(nrows=3, sharex='all', sharey='all') 65 | ax[0].loglog(freq_hz, Ppqr_Dryden[0], label = "Dryden - Level: " + level) 66 | ax[0].loglog(freq_hz, Ppqr_VonKarman[0], label = "VonKarman - Level: " + level) 67 | ax[0].set_ylabel("Disturbance p (rad/s)"); ax[0].legend(); ax[0].grid(True) 68 | 69 | ax[1].loglog(freq_hz, Ppqr_Dryden[1], label = "Dryden - Level: " + level) 70 | ax[1].loglog(freq_hz, Ppqr_VonKarman[1], label = "VonKarman - Level: " + level) 71 | ax[1].set_ylabel("Disturbance q (rad/s)"); ax[1].legend(); ax[1].grid(True) 72 | 73 | ax[2].loglog(freq_hz, -Ppqr_Dryden[2], label = "Dryden - Level: " + level) 74 | ax[2].loglog(freq_hz, -Ppqr_VonKarman[2], label = "VonKarman - Level: " + level) 75 | ax[2].set_xlabel("Frequency (Hz)"); ax[2].set_ylabel("Disturbance r (rad/s)"); ax[2].legend(); ax[2].grid(True) 76 | ax[2].set_xlim([0.1, 50]) 77 | fig.suptitle("Turbulence Spectrum") 78 | -------------------------------------------------------------------------------- /Core/Servo.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | 12 | #%% Servo Model 13 | # Limit 14 | def Limit(x, xLim): 15 | if abs(x) > xLim: 16 | x = np.sign(x) * xLim 17 | return x 18 | 19 | # Freeplay 20 | def Freeplay(x, xOut, freeplay): 21 | if (x > xOut): 22 | xOut = max(xOut, x - freeplay/2); 23 | elif (x < xOut): 24 | xOut = min(xOut, x + freeplay/2); 25 | return xOut 26 | 27 | class Servo: 28 | def __init__(self, dt, freqNat_rps, damp, timeDelay_s = 0, cmdLim = np.Inf, pwrLim = np.Inf, aLim = np.Inf, vLim = np.Inf, pLim = np.Inf, freeplay = 0.0): 29 | self.dt = dt 30 | self.timeDelay_s = timeDelay_s 31 | self.freqNat_rps = freqNat_rps 32 | self.damp = damp 33 | 34 | self.cmdLim = cmdLim 35 | self.pwrLim = pwrLim # Specific Power Pwr/J = v * a = A**2 * freqNat_rps**3 36 | self.aLim = aLim 37 | self.vLim = vLim 38 | self.pLim = pLim 39 | 40 | self.freeplay = freeplay 41 | 42 | 43 | def Start(self): 44 | self.cmd = 0.0 45 | cmdQueue = np.zeros(max(int(self.timeDelay_s/self.dt), 1)) 46 | self.cmdQueue = cmdQueue 47 | self.vState = 0.0 48 | self.pState = 0.0 49 | self.pwr = 0.0 50 | self.a = 0.0 51 | self.v = 0.0 52 | self.p = 0.0 53 | self.pOut = 0.0 54 | 55 | 56 | def Update(self, cmd): 57 | # Command Limit 58 | 59 | # Apply the time delay as a queue 60 | self.cmdQueue[:-1] = self.cmdQueue[1:] 61 | self.cmdQueue[-1] = cmd 62 | 63 | self.cmd = Limit(self.cmdQueue[0], self.cmdLim) 64 | 65 | # Tracking Error 66 | e = self.p - self.cmd 67 | 68 | # Compute Acceleration 69 | self.a = -(2*self.damp*self.freqNat_rps * self.vState + self.freqNat_rps**2 * e) 70 | 71 | # Acceleration Limit 72 | self.a = Limit(self.a, self.aLim) 73 | 74 | # Specific Power Limit (pwr/J = a * v) 75 | # pwr = self.a * self.vState 76 | self.pwr = self.a * (self.vState + self.a * self.dt) 77 | if abs(self.pwr) > self.pwrLim: 78 | self.a = np.sign(self.a) * abs(self.pwrLim / self.vState) 79 | 80 | # Integrate Acceleration to update Velocity 81 | self.v = self.vState + self.a * self.dt 82 | 83 | # Rate Limit 84 | self.v = Limit(self.v, self.vLim) 85 | self.a = (self.v - self.vState) / self.dt 86 | 87 | # Compute specific power again 88 | self.pwr = self.a * self.v 89 | 90 | # Integrate Velocity to update Position 91 | self.p = self.pState + self.v * self.dt 92 | 93 | # Freeplay 94 | self.pOut = Freeplay(self.p, self.pOut, self.freeplay) 95 | 96 | # Position Limit 97 | # self.pOut = Limit(self.p, self.pLim) 98 | self.pOut = Limit(self.pOut, self.pLim) 99 | self.v = (self.p - self.pState) / self.dt 100 | 101 | 102 | self.pState = self.pOut 103 | self.vState = self.v 104 | 105 | 106 | return self.pOut 107 | -------------------------------------------------------------------------------- /Tests/TestRobustStab_SP_8_9.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Example script for testing Frequency Response Estimation - MIMO. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import control 15 | 16 | # Hack to allow loading the Core package 17 | if __name__ == "__main__" and __package__ is None: 18 | from sys import path, argv 19 | from os.path import dirname, abspath, join 20 | 21 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 22 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 23 | 24 | del path, argv, dirname, abspath, join 25 | 26 | from Core import FreqTrans 27 | 28 | 29 | # Constants 30 | hz2rps = 2*np.pi 31 | rps2hz = 1/hz2rps 32 | 33 | rad2deg = 180/np.pi 34 | deg2rad = 1/rad2deg 35 | 36 | 37 | #%% Define a linear plant systems 38 | omega = np.logspace(-3,2,101) 39 | tau = 75 40 | 41 | G = control.tf([[[0, -87.8], [0, 1.4]], [[0, -108.2], [0, -1.4]]], [[[tau, 1], [tau, 1]], [[tau, 1], [tau, 1]]]) 42 | K = control.tf([[[-0.0015*tau, -0.0015*1], [0]], [[0], [-0.075*tau, -0.075*1]]], [[[1, 1e-6], [1]], [[1], [1, 1e-6]]]) 43 | wI = 0.2 * control.tf([5, 1],[0.5, 1]) 44 | 45 | G_frd = control.frd(G, omega) 46 | K_frd = control.frd(K, omega) 47 | Wi_frd = control.frd(wI, omega) 48 | 49 | Wi_fresp = Wi_frd.fresp[0,0] 50 | WiAbs_fresp = np.abs(Wi_fresp) 51 | 52 | Li = K * G 53 | Li_frd = control.frd(Li, omega) 54 | 55 | Si_frd = control.FRD(Li_frd.fresp, omega) 56 | for iFreq in range(len(omega)): 57 | Si_frd.fresp[..., iFreq] = np.linalg.inv(np.eye(2) + Li_frd.fresp[..., iFreq]) 58 | 59 | Ti_frd = Si_frd * Li_frd 60 | 61 | svTi = FreqTrans.Sigma(Ti_frd.fresp) 62 | 63 | 64 | # Compute strucuted singular value 65 | Delta = np.ones((2,2), dtype=float) 66 | muTi_bnds, muTi_info = FreqTrans.SigmaStruct(Ti_frd.fresp, Delta, bound = 'upper') 67 | 68 | 69 | #%% 70 | plt.figure('S&P Example 8.9 (Figure 8.11)') 71 | plt.plot(omega, 1/WiAbs_fresp, '--r', label = '1/|Wi|}') 72 | plt.plot(omega, muTi_bnds.T, 'b', label = 'mu(Ti)') 73 | plt.plot(omega, svTi[0], 'g', label = 'max sv(Ti)') 74 | plt.plot(omega, svTi[1], ':g', label = 'min sv(Ti)') 75 | 76 | plt.xlabel('Frequency [rad/sec]') 77 | plt.ylabel('Magnitude [-]') 78 | plt.xscale('log') 79 | plt.yscale('log') 80 | plt.grid(True) 81 | plt.legend() 82 | 83 | #%% 84 | M_fresp = Ti_frd.fresp * Wi_frd.fresp 85 | 86 | svM = FreqTrans.Sigma(M_fresp) 87 | svM_max = np.max(svM, axis = 0) 88 | 89 | muM_bnds, muM_info = FreqTrans.SigmaStruct(M_fresp, Delta, bound = 'both') 90 | muM_min = np.min(muM_bnds, axis = 0) 91 | 92 | km = 1/muM_min 93 | 94 | mag2db = control.mag2db 95 | 96 | plt.figure('S&P Example 8.9 (Stability Margins)'); 97 | plt.semilogx(omega, mag2db(1/svM_max), 'g-', label = '1 / svM_max') 98 | plt.semilogx(omega, mag2db(1/muM_min), 'b-', label = '1 / muM_min') 99 | plt.semilogx(omega, mag2db(1) * np.ones_like(omega), 'r-', label = 'Crit = 0') 100 | plt.semilogx(omega, mag2db(1/0.4) * np.ones_like(omega), 'r--', label = 'Crit = 0.4') 101 | 102 | plt.xlabel('Frequency [rad/sec]') 103 | plt.ylabel('Stability Margin [dB]') 104 | plt.grid(True) 105 | plt.legend() 106 | -------------------------------------------------------------------------------- /Core/Systems.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | import control 12 | 13 | #%% Connect by strings 14 | def ConnectName(sysList, connectNames, inKeep, outKeep): 15 | 16 | sys = [] 17 | for s in sysList: 18 | if sys == []: 19 | sys = s 20 | else: 21 | sys = control.append(sys, control.ss(s)) 22 | 23 | inNames = [] 24 | outNames = [] 25 | for s in sysList: 26 | inNames += s.InputName 27 | outNames += s.OutputName 28 | 29 | Q = [[inNames.index(s)+1, outNames.index(s)+1] for s in connectNames] 30 | 31 | inputv = [inNames.index(s)+1 for s in inKeep] 32 | outputv = [outNames.index(s)+1 for s in outKeep] 33 | 34 | sysOut = control.connect(sys, Q, inputv, outputv) 35 | 36 | sysOut.InputName = inKeep 37 | sysOut.OutputName = outKeep 38 | 39 | return sysOut 40 | 41 | 42 | #%% Controller Models 43 | def PID2Exc(Kp = 1, Ki = 0, Kd = 0, b = 1, c = 1, Tf = 0, dt = None): 44 | # Inputs: ['ref', 'sens', 'exc'] 45 | # Outputs: ['cmd', 'ff', 'fb', 'exc'] 46 | 47 | sysR = control.tf2ss(control.tf([Kp*b*Tf + Kd*c, Kp*b + Ki*Tf, Ki], [Tf, 1, 0])) 48 | sysY = control.tf2ss(control.tf([Kp*Tf + Kd, Kp + Ki*Tf, Ki], [Tf, 1, 0])) 49 | sysX = control.tf2ss(control.tf(1,1)) # Excitation Input 50 | 51 | sys = control.append(sysR, sysY, sysX) 52 | 53 | sys.C = np.concatenate((sys.C[0,:] - sys.C[1,:] + sys.C[2,:], sys.C)) 54 | sys.D = np.concatenate((sys.D[0,:] - sys.D[1,:] + sys.D[2,:], sys.D)) 55 | 56 | sys.outputs = 4 57 | 58 | if dt is not None: 59 | sys = control.c2d(sys, dt) 60 | 61 | return sys 62 | 63 | def PID2(Kp = 1, Ki = 0.0, Kd = 0, b = 1, c = 1, Tf = 0, dt = None): 64 | # Inputs: ['ref', 'sens'] 65 | # Outputs: ['cmd'] 66 | 67 | sysR = control.tf2ss(control.tf([Kp*b*Tf + Kd*c, Kp*b + Ki*Tf, Ki], [Tf, 1, 0])) 68 | sysY = control.tf2ss(control.tf([Kp*Tf + Kd, Kp + Ki*Tf, Ki], [Tf, 1, 0])) 69 | 70 | sys = control.append(sysR, sysY) 71 | 72 | sys.C = sys.C[0,:] - sys.C[1,:] 73 | sys.D = sys.D[0,:] - sys.D[1,:] 74 | 75 | sys.outputs = 1 76 | 77 | if dt is not None: 78 | sys = control.c2d(sys, dt) 79 | 80 | return sys 81 | 82 | 83 | #%% Effector Models 84 | def ActuatorModel(bw, delay = (0, 1)): 85 | # Inputs: ['cmd'] 86 | # Outputs: ['pos'] 87 | 88 | sysNom = control.tf2ss(control.tf(1, [1/bw, 1])) 89 | 90 | delayPade = control.pade(delay[0], n=delay[1]) 91 | sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1])) 92 | 93 | sys = sysDelay * sysNom 94 | 95 | return sys 96 | 97 | 98 | #%% Sensor models 99 | def SensorModel(bw, delay = (0, 1)): 100 | # Inputs: ['meas', 'dist'] 101 | # Outputs: ['sens'] 102 | 103 | sysNom = control.tf2ss(control.tf(1, [1/bw, 1])) 104 | 105 | delayPade = control.pade(delay[0], n=delay[1]) 106 | sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1])) 107 | 108 | sysDist = control.tf2ss(control.tf(1, 1)) 109 | 110 | sys = control.append(sysDelay * sysNom, sysDist) 111 | sys.C = sys.C[0,:] + sys.C[1,:] 112 | sys.D = sys.D[0,:] + sys.D[1,:] 113 | 114 | sys.outputs = 1 115 | 116 | return sys 117 | -------------------------------------------------------------------------------- /Core/AirDataCalibration.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | 12 | import AirData 13 | 14 | # Constants 15 | 16 | #%% 17 | # Define the Cost Function for the Optimization 18 | def CostFunc(xOpt, optInfo, oDataList, param): 19 | 20 | # Unpack the optimization 'x' vector; vMean is the first 3 vals, the rest are param entries 21 | vWind = [] 22 | iEnd = 0 23 | for iSeg, seg in enumerate(optInfo['wind']): 24 | lenX = len(seg['val']) 25 | iStart = iEnd 26 | iEnd = iStart + lenX 27 | iSlice = slice(iStart, iEnd) 28 | vWind.append(xOpt[iSlice]) 29 | 30 | # Apply global free variables 31 | # param['pTip']['K'] = xOpt[iEnd] 32 | # param['pTip']['bias'] = xOpt[iEnd+1] 33 | param['v']['K'] = xOpt[iEnd] 34 | param['v']['bias'] = xOpt[iEnd+1] 35 | param['alpha']['K'] = xOpt[iEnd+2] 36 | param['alpha']['bias'] = xOpt[iEnd+3] 37 | param['beta']['K'] = xOpt[iEnd+4] 38 | param['beta']['bias'] = xOpt[iEnd+5] 39 | 40 | costList = [] 41 | for iSeg, oData in enumerate(oDataList): 42 | 43 | # Apply segment-wise free variables 44 | oData['vMean_AE_L_mps'] = vWind[iSeg] 45 | 46 | # Compute the Airspeed solution with error model parameters 47 | # calib = AirData.ApplyPressureCalibration(oData, param) 48 | calib = AirData.ApplyCalibration(oData, param) 49 | v_BA_B_mps, v_BA_L_mps = AirData.Airspeed2NED(calib['v_PA_P_mps'], oData['sB_L_rad'], param) 50 | 51 | # Compute the Ground Speeds 52 | # Wind Estimate, assume constant at mean value 53 | numSamp = v_BA_B_mps.shape[-1] 54 | v_AE_L_mps = np.repeat([oData['vMean_AE_L_mps']], numSamp, axis=0).T 55 | 56 | # Compute the Groudspeeds from the Corrected Airspeeds using the Wind Estimate 57 | vEst_BE_L_mps = v_AE_L_mps + v_BA_L_mps 58 | 59 | # Cost for each segment 60 | cost = np.linalg.norm(oData['vB_L_mps'] - vEst_BE_L_mps, 2) / numSamp 61 | costList.append(cost) 62 | 63 | # Combined Total Cost 64 | costTotal = np.mean(costList) 65 | 66 | # Display progress 67 | optInfo['Nfeval'].append(optInfo['Nfeval'][-1] + 1) 68 | optInfo['cost'].append(costTotal) 69 | if optInfo['Nfeval'][-1]%10 == 0: 70 | # plt.plot(optInfo['Nfeval'], optInfo['cost']) 71 | print('Nfeval: ', optInfo['Nfeval'][-1], 'Cost: ', optInfo['cost'][-1]) 72 | 73 | return costTotal 74 | 75 | 76 | #%% Air Calibration Wrapper 77 | def EstCalib(opt, oDataList, param): 78 | 79 | from scipy.optimize import minimize 80 | 81 | # Pack segment-wise free variables 82 | xOpt = [] 83 | xBnds = [] 84 | for iSeg, seg in enumerate(opt['wind']): 85 | for i in range(0, len(seg['val'])): 86 | xOpt.append(seg['val'][i]) 87 | xBnds.append([seg['lb'][i], seg['ub'][i]]) 88 | 89 | for p in opt['param']: 90 | xOpt.append(p['val']) 91 | xBnds.append([p['lb'], p['ub']]) 92 | 93 | 94 | # Initial Guess, based on assuming perfect calibration, constant wind. 95 | for iSeg, oData in enumerate(oDataList): 96 | if all(opt['wind'][iSeg]['val'] == 0.0) or all(opt['wind'][iSeg]['val'] == None): 97 | # Compute the Wind and take the mean 98 | calib = AirData.ApplyCalibration(oData, param) 99 | v_BA_B_mps, v_BA_L_mps = AirData.Airspeed2NED(calib['v_PA_P_mps'], oData['sB_L_rad'], param) 100 | 101 | # Compute the Mean of the Wind Estimate, in NED 102 | # Subtract the Estimated Body Airspeed from the Inertial Velocity 103 | #v_AE_L = v_BE_L - v_BA_L 104 | oData['vMean_AE_L_mps'] = np.mean(oData['vB_L_mps'] - v_BA_L_mps, axis=1) 105 | 106 | else: 107 | # Apply provided initial guess 108 | oData['vMean_AE_L_mps'] = opt['wind'][iSeg]['val'] 109 | 110 | 111 | # Optimization Info dict, pass to function 112 | optInfo = {} 113 | optInfo['wind'] = opt['wind'] 114 | optInfo['param'] = opt['param'] 115 | optInfo['Nfeval'] = [0] 116 | optInfo['cost'] = [0] 117 | opt['args'] = (optInfo, oDataList, param) 118 | 119 | # Run Optimization 120 | optResult = minimize(CostFunc, xOpt, bounds = xBnds, args = opt['args'], method = opt['Method'], options = opt['Options']) 121 | 122 | # Copy Results to xOpt 123 | xOpt = np.copy(optResult['x']) 124 | 125 | return optResult -------------------------------------------------------------------------------- /Tests/TestOMS.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Example script for generating Schroeder type Multisine Excitations. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # Hack to allow loading the Core package 16 | if __name__ == "__main__" and __package__ is None: 17 | from sys import path, argv 18 | from os.path import dirname, abspath, join 19 | 20 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 21 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 22 | 23 | del path, argv, dirname, abspath, join 24 | 25 | from Core import GenExcite 26 | from Core import FreqTrans 27 | 28 | 29 | # Constants 30 | hz2rps = 2*np.pi 31 | rps2hz = 1/hz2rps 32 | 33 | #%% Define the frequency selection and distribution of the frequencies into the signals 34 | numChan = 3 35 | freqRate_hz = 50; 36 | timeDur_s = 10.0 37 | numCycles = 1 38 | 39 | freqMinDes_rps = (numCycles/timeDur_s) * hz2rps * np.ones(numChan) 40 | #freqMaxDes_rps = (freqRate_hz/2) * hz2rps * np.ones(numChan) 41 | freqMaxDes_rps = 10 * hz2rps * np.ones(numChan) 42 | freqStepDes_rps = (10 / freqRate_hz) * hz2rps 43 | methodSW = 'zip' # "zippered" component distribution 44 | 45 | ## Generate MultiSine Frequencies 46 | freqElem_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW) 47 | timeDur_s = time_s[-1] - time_s[0] 48 | 49 | ## Generate Schroeder MultiSine Signal 50 | ampElem_nd = np.ones_like(freqElem_rps) ## Approximate relative signal amplitude, create flat 51 | sigList, phaseElem_rad, sigElem = GenExcite.MultiSine(freqElem_rps, ampElem_nd, sigIndx, time_s, costType = 'Norm2', phaseInit_rad = 0, boundPhase = 1, initZero = 1, normalize = 'peak'); 52 | 53 | 54 | ## Results 55 | peakFactor = GenExcite.PeakFactor(sigList) 56 | peakFactorRel = peakFactor / np.sqrt(2) 57 | print(peakFactorRel) 58 | 59 | # Signal Power 60 | sigPowerRel = (ampElem_nd / max(ampElem_nd))**2 / len(ampElem_nd) 61 | 62 | 63 | if True: 64 | fig, ax = plt.subplots(ncols=1, nrows=numChan, sharex=True) 65 | for iChan in range(0, numChan): 66 | ax[iChan].plot(time_s, sigList[iChan]) 67 | ax[iChan].set_ylabel('Amplitude (nd)') 68 | ax[iChan].grid(True) 69 | ax[iChan].set_xlabel('Time (s)') 70 | 71 | if True: 72 | fig, ax = plt.subplots(ncols=1, nrows=numChan, sharex=True) 73 | for iChan in range(0, numChan): 74 | for iElem in sigIndx[iChan]: 75 | ax[iChan].plot(time_s, sigElem[iElem]) 76 | ax[iChan].set_ylabel('Amplitude (nd)') 77 | ax[iChan].grid(True) 78 | ax[iChan].set_xlabel('Time (s)') 79 | 80 | 81 | #%% Plot the Excitation Spectrum 82 | 83 | ## Compute Spectrum of each channel 84 | optFFT = FreqTrans.OptSpect(freqRate = freqRate_hz * hz2rps, winType = ('tukey', 0.0), smooth = ('box', 1)) 85 | optCZT = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, winType = ('tukey', 0.0), smooth = ('box', 1)) 86 | 87 | freq_fft = [] 88 | P_dB_fft = [] 89 | freq_czt = [] 90 | P_dB_czt = [] 91 | 92 | for iChan, sig in enumerate(sigList): 93 | freq_rps_fft, _, P_fft = FreqTrans.Spectrum(sig, optFFT) 94 | freq_fft.append(freq_rps_fft * rps2hz) 95 | P_dB_fft.append(20*np.log10(P_fft)) 96 | 97 | optCZT.freq = freqElem_rps[sigIndx[iChan]] 98 | freq_rps_czt, _, P_czt = FreqTrans.Spectrum(sig, optCZT) 99 | freq_czt.append(freq_rps_czt * rps2hz) 100 | P_dB_czt.append(20*np.log10(P_czt)) 101 | 102 | nChan = len(P_dB_fft) 103 | 104 | plt.figure() 105 | for iChan in range(0, nChan): 106 | plt.subplot(nChan, 1, iChan+1) 107 | plt.plot(freq_fft[iChan].T, P_dB_fft[iChan].T, '-k', label='FFT Pxx') 108 | plt.plot(freq_czt[iChan].T, P_dB_czt[iChan].T, '.r-', label='CZT Pxx') 109 | plt.grid() 110 | plt.ylabel('Spectrum (dB)'); 111 | plt.xlim([0, 12]); 112 | 113 | plt.xlabel('frequency (Hz)'); 114 | plt.legend() 115 | plt.show() 116 | 117 | 118 | #%% Create the output for JSON config 119 | 120 | timeFinal_s = time_s[-1] 121 | timeStart_s = time_s[0] 122 | 123 | jsonMulti = {} 124 | for iChan in range(0, numChan): 125 | iElem = sigIndx[iChan] 126 | 127 | dictChan = {} 128 | dictChan['Type'] = 'MultiSine' 129 | dictChan['Duration'] = timeDur_s 130 | dictChan['Frequency'] = list(freqElem_rps[iElem]) 131 | dictChan['Phase'] = list(phaseElem_rad[iElem]) 132 | dictChan['Amplitude'] = list(ampElem_nd[iElem]) 133 | 134 | nameChan = 'OMS_' + str(iChan+1) 135 | 136 | jsonMulti[nameChan] = dictChan 137 | 138 | import json 139 | #print(json.dumps(jsonMulti, separators=(', ', ': '))) 140 | 141 | -------------------------------------------------------------------------------- /Tests/TestFreqRespEst.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Example script for testing Frequency Response Estimation - SISO. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import scipy.signal as signal 15 | import control 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import GenExcite 28 | from Core import FreqTrans 29 | 30 | 31 | # Constants 32 | hz2rps = 2*np.pi 33 | rps2hz = 1/hz2rps 34 | 35 | rad2deg = 180/np.pi 36 | deg2rad = 1/rad2deg 37 | 38 | mag2db = control.mag2db 39 | db2mag = control.db2mag 40 | 41 | 42 | #%% Define a linear system 43 | freqRate_hz = 50.0 44 | freqRate_rps = freqRate_hz * hz2rps 45 | 46 | wn = 3 * hz2rps 47 | d = 0.2 48 | 49 | sys = signal.TransferFunction([wn**2], [1, 2*d*wn, wn**2]) 50 | freqLin_rps = np.fft.rfftfreq(1000, 1/freqRate_rps) 51 | 52 | freqLin_rps, Txy = signal.freqresp(sys, w=freqLin_rps) 53 | freqLin_rps, gainLin_dB, phaseLin_deg = signal.bode(sys, w=freqLin_rps) 54 | freqLin_hz = freqLin_rps * rps2hz 55 | 56 | 57 | #%% Chirp signal with FFT 58 | freqInit_rps = 0.1 * hz2rps 59 | freqFinal_rps = 20 * hz2rps 60 | timeDur_s = 10.0 61 | ampInit = 1.0 62 | ampFinal = ampInit 63 | 64 | time_s = np.linspace(0.0, timeDur_s, int(timeDur_s * freqRate_hz) + 1) 65 | 66 | # Generate the chirp 67 | x, ampChirpX, freqChirp_rps = GenExcite.Chirp(freqInit_rps, freqFinal_rps, time_s, ampInit, ampFinal, freqType = 'linear', ampType = 'linear', initZero = 1) 68 | 69 | # Simulate the excitation through the system 70 | time_s, y, _ = signal.lsim(sys, x, time_s) 71 | y = np.atleast_2d(y) 72 | 73 | # Estimate the transfer function 74 | optSpec = FreqTrans.OptSpect(freqRate = freqRate_rps, smooth = ('box', 1), winType=('tukey', 0.0)) 75 | freq_rps, Txy, Cxy, Pxx, Pyy, Pxy = FreqTrans.FreqRespFuncEst(x, y, optSpec) 76 | gain_dB, phase_deg = FreqTrans.GainPhase(Txy) 77 | freq_hz = freq_rps * rps2hz 78 | 79 | freq_hz = np.squeeze(freq_hz) 80 | gain_dB = np.squeeze(gain_dB) 81 | phase_deg = np.unwrap(np.squeeze(phase_deg) * deg2rad) * rad2deg 82 | Cxy = np.squeeze(Cxy) 83 | 84 | 85 | #%% Multisine signal with CZT 86 | numChan = 1 87 | numCycles = 1 88 | 89 | freqMinDes_rps = freqInit_rps * np.ones(numChan) 90 | freqMaxDes_rps = freqFinal_rps * np.ones(numChan) 91 | freqStepDes_rps = (10/freqRate_hz) * hz2rps 92 | methodSW = 'zip' # "zippered" component distribution 93 | 94 | # Generate MultiSine Frequencies 95 | freqElem_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW) 96 | 97 | # Generate Schroeder MultiSine Signal 98 | ampElem_nd = np.linspace(ampInit, ampInit, len(freqElem_rps)) / np.sqrt(len(freqElem_rps)) 99 | x, _, sigElem = GenExcite.MultiSine(freqElem_rps, ampElem_nd, sigIndx, time_s, phaseInit_rad = 0, boundPhase = 1, initZero = 1, normalize = 'peak', costType = 'Schroeder'); 100 | 101 | 102 | # Simulate the excitation through the system 103 | time_s, y, _ = signal.lsim(sys, x.squeeze(), time_s) 104 | y = np.atleast_2d(y) 105 | 106 | # Estimate the transfer function 107 | optSpectCzt = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, freq = freqElem_rps, smooth = ('box', 1), winType=('tukey', 0.0)) 108 | freq_czt_rps, Txy_czt, Cxy_czt, Pxx_czt, Pyy_czt, Pxy_czt = FreqTrans.FreqRespFuncEst(x, y, optSpectCzt) 109 | gain_czt_dB, phase_czt_deg = FreqTrans.GainPhase(Txy_czt) 110 | freq_czt_hz = freq_czt_rps * rps2hz 111 | 112 | # Plot 113 | freq_czt_hz = np.squeeze(freq_czt_hz) 114 | gain_czt_dB = np.squeeze(gain_czt_dB) 115 | phase_czt_deg = np.squeeze(phase_czt_deg) 116 | Cxy_czt = np.squeeze(Cxy_czt) 117 | 118 | 119 | #%% 120 | plt.figure(1) 121 | ax1 = plt.subplot(3,1,1); plt.grid(True) 122 | ax1.semilogx(freqLin_hz, gainLin_dB, 'b-') 123 | ax1.semilogx(freq_hz, gain_dB, '.g--') 124 | ax1.semilogx(freq_czt_hz, gain_czt_dB, '*r') 125 | ax2 = plt.subplot(3,1,2); plt.grid(True) 126 | ax2.semilogx(freqLin_hz, phaseLin_deg, 'b-'); plt.ylim([-180, 180]); 127 | ax2.semilogx(freq_hz, phase_deg, '.g--') 128 | ax2.semilogx(freq_czt_hz, phase_czt_deg, '*r') 129 | ax3 = plt.subplot(3,1,3); plt.grid(True) 130 | ax3.semilogx(freqLin_hz, np.ones_like(freqLin_hz), 'b-', label = 'Linear Model'); plt.ylim([0, 1.2]) 131 | ax3.semilogx(freq_hz, Cxy, '.g--', label = 'FFT Estimate') 132 | ax3.semilogx(freq_czt_hz, Cxy_czt, '*r', label = 'CZT Estimate') 133 | 134 | ax3.legend() 135 | -------------------------------------------------------------------------------- /Tests/TestSchroeder.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Example script for generating Schroeder type Multisine Excitations. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # Hack to allow loading the Core package 16 | if __name__ == "__main__" and __package__ is None: 17 | from sys import path, argv 18 | from os.path import dirname, abspath, join 19 | 20 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 21 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 22 | 23 | del path, argv, dirname, abspath, join 24 | 25 | from Core import GenExcite 26 | from Core import FreqTrans 27 | 28 | 29 | # Constants 30 | hz2rps = 2*np.pi 31 | rps2hz = 1/hz2rps 32 | 33 | #%% Define the frequency selection and distribution of the frequencies into the signals 34 | numChan = 3 35 | freqRate_hz = 50; 36 | timeDur_s = 10.0 37 | numCycles = 1 38 | 39 | freqMinDes_rps = (numCycles/timeDur_s) * hz2rps * np.ones(numChan) 40 | #freqMaxDes_rps = (freqRate_hz/2) * hz2rps * np.ones(numChan) 41 | freqMaxDes_rps = 10 * hz2rps * np.ones(numChan) 42 | freqStepDes_rps = (10 / freqRate_hz) * hz2rps 43 | methodSW = 'zip' # "zippered" component distribution 44 | 45 | ## Generate MultiSine Frequencies 46 | freqElem_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW) 47 | timeDur_s = time_s[-1] - time_s[0] 48 | 49 | ## Generate Schroeder MultiSine Signal 50 | ampElem_nd = np.ones_like(freqElem_rps) ## Approximate relative signal amplitude, create flat 51 | sigList, phaseElem_rad, sigElem = GenExcite.MultiSine(freqElem_rps, ampElem_nd, sigIndx, time_s, costType = 'Schroeder', phaseInit_rad = 0, boundPhase = True, initZero = True, normalize = 'peak'); 52 | 53 | 54 | ## Results 55 | peakFactor = GenExcite.PeakFactor(sigList) 56 | peakFactorRel = peakFactor / np.sqrt(2) 57 | print(peakFactorRel) 58 | 59 | # Signal Power 60 | sigPowerRel = (ampElem_nd / max(ampElem_nd))**2 / len(ampElem_nd) 61 | 62 | 63 | 64 | if True: 65 | fig, ax = plt.subplots(ncols=1, nrows=numChan, sharex=True) 66 | for iChan in range(0, numChan): 67 | ax[iChan].plot(time_s, sigList[iChan]) 68 | ax[iChan].set_ylabel('Amplitude (nd)') 69 | ax[iChan].grid(True) 70 | ax[iChan].set_xlabel('Time (s)') 71 | 72 | if True: 73 | fig, ax = plt.subplots(ncols=1, nrows=numChan, sharex=True) 74 | for iChan in range(0, numChan): 75 | for iElem in sigIndx[iChan]: 76 | ax[iChan].plot(time_s, sigElem[iElem]) 77 | ax[iChan].set_ylabel('Amplitude (nd)') 78 | ax[iChan].grid(True) 79 | ax[iChan].set_xlabel('Time (s)') 80 | 81 | 82 | #%% Plot the Excitation Spectrum 83 | 84 | ## Compute Spectrum of each channel 85 | optFFT = FreqTrans.OptSpect(freqRate = freqRate_hz * hz2rps) 86 | optMAT = FreqTrans.OptSpect(dftType = 'dftmat', freqRate = freqRate_hz * hz2rps) 87 | optCZT = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps) 88 | 89 | freq_fft = [] 90 | P_dB_fft = [] 91 | freq_mat = [] 92 | P_dB_mat = [] 93 | freq_czt = [] 94 | P_dB_czt = [] 95 | 96 | for iChan, sig in enumerate(sigList): 97 | freq_rps_fft, _, P_fft = FreqTrans.Spectrum(sig, optFFT) 98 | freq_fft.append(freq_rps_fft * rps2hz) 99 | P_dB_fft.append(20*np.log10(P_fft)) 100 | 101 | optMAT.freq = freqElem_rps[sigIndx[iChan]] 102 | freq_rps_mat, _, P_mat = FreqTrans.Spectrum(sig, optMAT) 103 | freq_mat.append(freq_rps_mat * rps2hz) 104 | P_dB_mat.append(20*np.log10(P_mat)) 105 | 106 | optCZT.freq = freqElem_rps[sigIndx[iChan]] 107 | freq_rps_czt, _, P_czt = FreqTrans.Spectrum(sig, optCZT) 108 | freq_czt.append(freq_rps_czt * rps2hz) 109 | P_dB_czt.append(20*np.log10(P_czt)) 110 | 111 | nChan = len(P_dB_fft) 112 | 113 | plt.figure() 114 | for iChan in range(0, nChan): 115 | plt.subplot(nChan, 1, iChan+1) 116 | plt.plot(freq_fft[iChan].T, P_dB_fft[iChan].T, '-k', label='FFT Pxx') 117 | plt.plot(freq_mat[iChan].T, P_dB_mat[iChan].T, '*b--', label='MAT Pxx') 118 | plt.plot(freq_czt[iChan].T, P_dB_czt[iChan].T, '.r-', label='CZT Pxx') 119 | plt.grid() 120 | plt.ylabel('Spectrum (dB)'); 121 | # plt.xlim([0, 12]); 122 | 123 | plt.xlabel('frequency (Hz)'); 124 | plt.legend() 125 | plt.show() 126 | 127 | 128 | #%% Create the output for JSON config 129 | 130 | timeFinal_s = time_s[-1] 131 | timeStart_s = time_s[0] 132 | 133 | jsonMulti = {} 134 | for iChan in range(0, numChan): 135 | iElem = sigIndx[iChan] 136 | 137 | dictChan = {} 138 | dictChan['Type'] = 'MultiSine' 139 | dictChan['Duration'] = timeDur_s 140 | dictChan['Frequency'] = list(freqElem_rps[iElem]) 141 | dictChan['Phase'] = list(phaseElem_rad[iElem]) 142 | dictChan['Amplitude'] = list(ampElem_nd[iElem]) 143 | 144 | nameChan = 'OMS_' + str(iChan+1) 145 | 146 | jsonMulti[nameChan] = dictChan 147 | 148 | import json 149 | print(json.dumps(jsonMulti, separators=(', ', ': '))) 150 | -------------------------------------------------------------------------------- /Examples/HuginnLanding.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # Constants 32 | hz2rps = 2 * np.pi 33 | rps2hz = 1 / hz2rps 34 | 35 | #%% File Lists 36 | import os.path as path 37 | 38 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 39 | #pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Huginn') 40 | #pathBase = path.join('D:/', 'Huginn') 41 | 42 | fileList = {} 43 | flt = 'FLT02' 44 | fileList[flt] = {} 45 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 46 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 47 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 48 | 49 | fileList = {} 50 | flt = 'FLT03' 51 | fileList[flt] = {} 52 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 53 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 54 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 55 | 56 | flt = 'FLT04' 57 | fileList[flt] = {} 58 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 59 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 60 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 61 | 62 | flt = 'FLT05' 63 | fileList[flt] = {} 64 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 65 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 66 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 67 | 68 | flt = 'FLT06' 69 | fileList[flt] = {} 70 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 71 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 72 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 73 | 74 | 75 | #%% Wind/Air Cal 76 | landSegList = [ 77 | # {'flt': 'FLT03', 'seg': ('time_us', [950000000 - 1000.0e3, 970000000 - 1000.0e3], 'FLT03 - Approach')}, 78 | {'flt': 'FLT03', 'seg': ('time_us', [1016000000, 1036000000], 'FLT03')}, 79 | {'flt': 'FLT04', 'seg': ('time_us', [1180000000 + 20.0e3, 1200000000 + 20.0e3], 'FLT04')}, 80 | {'flt': 'FLT05', 'seg': ('time_us', [953000000, 973000000], 'FLT05')}, 81 | {'flt': 'FLT06', 'seg': ('time_us', [1248000000 - 60e3, 1268000000 - 60e3], 'FLT06')}, 82 | ] 83 | 84 | 85 | oDataLandList = [] 86 | for landSeg in landSegList: 87 | fltNum = landSeg['flt'] 88 | 89 | fileLog = fileList[fltNum]['log'] 90 | fileConfig = fileList[fltNum]['config'] 91 | 92 | oData, h5Data = Loader.Log_RAPTRS(fileLog, fileConfig) 93 | oDataLandList.append(OpenData.Segment(oData, landSeg['seg'])) 94 | 95 | 96 | #%% 97 | fig, ax = plt.subplots(nrows=5, sharex=True) 98 | for oDataLand in oDataLandList: 99 | 100 | latGps_deg = oDataLand['rGps_D_ddm'][0] 101 | lonGps_deg = oDataLand['rGps_D_ddm'][1] 102 | latB_deg = oDataLand['rB_D_ddm'][0] 103 | lonB_deg = oDataLand['rB_D_ddm'][1] 104 | # ax[0].plot(lonGps_deg, latGps_deg, '.', label='GPS') 105 | # ax[0].plot(lonB_deg, latB_deg, label='Ekf') 106 | ax[0].plot(oDataLand['time_s'] - oDataLand['time_s'][0], oDataLand['altBaro_m'], label = oDataLand['Desc']) 107 | ax[0].set_ylabel('Altitude (m)') 108 | ax[0].grid(True) 109 | 110 | ax[1].plot(oDataLand['time_s'] - oDataLand['time_s'][0], oDataLand['vIas_mps'], label = oDataLand['Desc']) 111 | ax[1].plot(oDataLand['time_s'] - oDataLand['time_s'][0], 15.7 * np.ones_like(oDataLand['vIas_mps']), 'r:', label = oDataLand['Desc']) 112 | ax[1].set_ylabel('Airspeed (m/s)') 113 | ax[1].grid(True) 114 | 115 | ax[2].plot(oDataLand['time_s'] - oDataLand['time_s'][0], oDataLand['Effectors']['cmdMotor_nd'], label = oDataLand['Desc']) 116 | ax[2].set_ylabel('Throttle (nd)') 117 | ax[2].grid(True) 118 | 119 | ax[3].plot(oDataLand['time_s'] - oDataLand['time_s'][0], oDataLand['refTheta_rad'] * 180.0/np.pi, label = oDataLand['Desc']) 120 | ax[3].set_ylabel('Theta Cmd (deg)') 121 | ax[3].grid(True) 122 | 123 | ax[4].plot(oDataLand['time_s'] - oDataLand['time_s'][0], oDataLand['sB_L_rad'][1] * 180.0/np.pi, label = oDataLand['Desc']) 124 | ax[4].set_ylabel('Theta Meas (deg)') 125 | ax[4].grid(True) 126 | ax[4].legend(loc = 'center left') 127 | -------------------------------------------------------------------------------- /Examples/HuginnBend.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Wed Jul 17 10:51:47 2019 5 | 6 | @author: rega0051 7 | """ 8 | 9 | """ 10 | University of Minnesota 11 | Aerospace Engineering and Mechanics - UAV Lab 12 | Copyright 2019 Regents of the University of Minnesota 13 | See: LICENSE.md for complete license details 14 | 15 | Author: Chris Regan 16 | 17 | Analysis for Huginn (mAEWing2) 18 | """ 19 | 20 | #%% 21 | # Import Libraries 22 | import numpy as np 23 | import matplotlib.pyplot as plt 24 | 25 | # Hack to allow loading the Core package 26 | if __name__ == "__main__" and __package__ is None: 27 | from sys import path, argv 28 | from os.path import dirname, abspath, join 29 | 30 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 31 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 32 | 33 | del path, argv, dirname, abspath, join 34 | 35 | from Core import Loader 36 | from Core import OpenData 37 | 38 | 39 | # Constants 40 | hz2rps = 2 * np.pi 41 | rps2hz = 1 / hz2rps 42 | 43 | #%% File Lists 44 | import os.path as path 45 | 46 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 47 | #pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Huginn') 48 | #pathBase = path.join('D:/', 'Huginn') 49 | 50 | fileList = {} 51 | #flt = 'FLT03' 52 | #fileList[flt] = {} 53 | #fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 54 | #fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 55 | #fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 56 | # 57 | #flt = 'FLT04' 58 | #fileList[flt] = {} 59 | #fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 60 | #fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 61 | #fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 62 | 63 | flt = 'FLT05' 64 | fileList[flt] = {} 65 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 66 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 67 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 68 | 69 | flt = 'FLT06' 70 | fileList[flt] = {} 71 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 72 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 73 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 74 | 75 | 76 | #%% Bending 77 | 78 | ExcName = 'Bend' 79 | segList = [] 80 | oDataList = [] 81 | 82 | for flt in fileList.keys(): 83 | fileLog = fileList[flt]['log'] 84 | fileConfig = fileList[flt]['config'] 85 | fileDef = fileList[flt]['def'] 86 | 87 | fltDef = Loader.JsonRead(fileDef) 88 | 89 | for testPt in fltDef['Test-Points']: 90 | if testPt['Excitation'] == ExcName: 91 | 92 | # Load Flight Log 93 | oData, h5Data = Loader.Log_RAPTRS(fileLog, fileConfig) 94 | t0 = testPt['time_us'][0] * 1e-6 95 | tf = t0 + (2*np.pi) + 2.0 96 | 97 | ODataSeg = OpenData.Segment(oData, ('time_s', [t0, tf])) 98 | oDataList.append(ODataSeg) 99 | 100 | seg = {'flt': flt, 'seg': ('time_us', testPt['time_us']), 'Desc': '{:.2f}'.format(ODataSeg['vIas_mps'].mean()) + ' m/s'} 101 | 102 | segList.append(seg) 103 | 104 | 105 | # Add Description to each segment 106 | #segList = 107 | # [{'flt': 'FLT03', 'seg': ('time_us', [645277887, 669278251]), 'Desc': '23.54 m/s'}, 108 | # {'flt': 'FLT03', 'seg': ('time_us', [828338096, 839997973]), 'Desc': '19.78 m/s'}, 109 | # {'flt': 'FLT04', 'seg': ('time_us', [872601981, 882883356]), 'Desc': '25.76 m/s'}, 110 | # {'flt': 'FLT05', 'seg': ('time_us', [551298711, 566483686]), 'Desc': '25.83 m/s'}, 111 | # {'flt': 'FLT05', 'seg': ('time_us', [766364351, 788467105]), 'Desc': '28.64 m/s'}, 112 | # {'flt': 'FLT06', 'seg': ('time_us', [929895366, 940358346]), 'Desc': '32.47 m/s'}, 113 | # {'flt': 'FLT06', 'seg': ('time_us', [1087597269, 1103958408]), 'Desc': '23.50 m/s'}] 114 | 115 | 116 | #%% 117 | from scipy.signal import filtfilt, butter 118 | b, a = butter(2, 0.1) 119 | 120 | 121 | fig, ax = plt.subplots(nrows=6, sharex=True) 122 | fig2, ax2 = plt.subplots(nrows=1, sharex=True) 123 | for oData in oDataList: 124 | 125 | pwrBat_w = oData['pwrPropLeft_V'] * oData['pwrPropLeft_A'] + oData['pwrPropRight_V'] * oData['pwrPropRight_A'] 126 | eBat_J = np.cumsum(pwrBat_w) * np.median(np.diff(oData['time_s'])) 127 | 128 | pwrBatFilt_w = filtfilt(b, a, pwrBat_w) 129 | eBatFilt_J = filtfilt(b, a, eBat_J) 130 | 131 | vGrnd_mps = np.linalg.norm(oData['vB_L_mps'], axis=0) 132 | aGrnd_mps2 = filtfilt(b, a, np.linalg.norm(oData['aB_I_mps2'], axis=0)) 133 | TE = 18 * (0.5 * vGrnd_mps**2 + oData['rB_D_ddm'][2]) 134 | 135 | hDot_mps = oData['vB_L_mps'][2] 136 | dTE = 18 * (9.81 * hDot_mps + aGrnd_mps2 * vGrnd_mps) 137 | 138 | nu = 1.0 139 | 140 | 141 | D = (dTE + nu * pwrBatFilt_w) / oData['vIas_mps'] 142 | 143 | 144 | ax[0].plot(oData['time_s'] - oData['time_s'][0], oData['Control']['cmdBend_nd']) 145 | ax[0].set_ylabel('Bend Cmd (nd)') 146 | ax[0].grid(True) 147 | 148 | ax[1].plot(oData['time_s'] - oData['time_s'][0], oData['altBaro_m']) 149 | ax[1].set_ylabel('Altitude (m)') 150 | ax[1].grid(True) 151 | 152 | ax[2].plot(oData['time_s'] - oData['time_s'][0], oData['vIas_mps']) 153 | # ax[2].plot(oData['time_s'] - oData['time_s'][0], 15.0 * np.ones_like(oData['vIas_mps']), 'r:') 154 | ax[2].set_ylabel('Airspeed (m/s)') 155 | ax[2].grid(True) 156 | 157 | ax[3].plot(oData['time_s'] - oData['time_s'][0], TE - TE[0] + eBatFilt_J) 158 | ax[3].set_ylabel('Energy (J)') 159 | ax[3].grid(True) 160 | 161 | ax[4].plot(oData['time_s'] - oData['time_s'][0], D - D[0]) 162 | ax[4].set_ylabel('delDrag Est (N)') 163 | ax[4].grid(True) 164 | 165 | ax[5].plot(oData['time_s'] - oData['time_s'][0], pwrBatFilt_w) 166 | ax[5].set_ylabel('Power Bat (Watt)') 167 | ax[5].grid(True) 168 | ax[5].legend(loc = 'center left') 169 | 170 | ax2.plot(oData['Control']['cmdBend_nd'], TE - TE[0], '*') 171 | -------------------------------------------------------------------------------- /Core/OpenData.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Dec 28 15:05:36 2018 5 | 6 | @author: louismueller 7 | """ 8 | 9 | # import 10 | import numpy as np 11 | 12 | #%% 13 | def PlotOverview(oData): 14 | import matplotlib.pyplot as plt 15 | from mpl_toolkits.mplot3d import Axes3D 16 | 17 | # Overview Plots 18 | # Find interesting stuff: takeoff, landing, excitations etc. 19 | time_s = oData['time_s'] 20 | 21 | # Airspeed 22 | vIas_mps = oData['vIas_mps'] 23 | vGps_mps = np.linalg.norm(oData['vGps_L_mps'], 2, axis=0) 24 | vB_mps = np.linalg.norm(oData['vB_L_mps'], 2, axis=0) 25 | fig0, ax0 = plt.subplots() 26 | ax0.plot(time_s, oData['refV_mps'], label='ref') 27 | ax0.plot(time_s, vIas_mps, label='airspeed') 28 | ax0.plot(time_s, vGps_mps, '.', label='Gps') 29 | ax0.plot(time_s, vB_mps, label='Ekf') 30 | ax0.grid() 31 | ax0.set_xlabel('Time (s)') 32 | ax0.set_ylabel('Airspeed (m/s)') 33 | ax0.set_title('Air Data Airspeed') 34 | ax0.legend() 35 | 36 | # Altitude 37 | altBaro_m = oData['altBaro_m'] 38 | altGps_m = oData['rGps_D_ddm'][2] 39 | altB_m = oData['rB_D_ddm'][2] 40 | fig1, ax1 = plt.subplots() 41 | ax1.plot(time_s, oData['refH_m'], label='ref') 42 | ax1.plot(time_s, altBaro_m, label='Baro') 43 | ax1.plot(time_s, altGps_m, '.', label='GPS') 44 | ax1.plot(time_s, altB_m - altB_m, label='Ekf') 45 | ax1.grid() 46 | ax1.set_xlabel('Time (s)') 47 | ax1.set_ylabel('Altitude (m)') 48 | ax1.set_title('Altitude') 49 | ax1.legend() 50 | 51 | # X and Y Position 52 | latGps_deg = oData['rGps_D_ddm'][0] 53 | lonGps_deg = oData['rGps_D_ddm'][1] 54 | latB_deg = oData['rB_D_ddm'][0] 55 | lonB_deg = oData['rB_D_ddm'][1] 56 | fig2, ax2 = plt.subplots() 57 | ax2.plot(lonGps_deg, latGps_deg, '.', label='GPS') 58 | ax2.plot(lonB_deg, latB_deg, label='Ekf') 59 | ax2.grid() 60 | ax2.axis('equal') 61 | ax2.set_xlabel('Longitude (deg)') 62 | ax2.set_ylabel('Latitude (deg)') 63 | ax2.set_title('Latitude and Longitude') 64 | ax2.legend() 65 | 66 | # Voltage 67 | pwrFmu_V = oData['pwrFmu_V'] 68 | fig3, ax3 = plt.subplots() 69 | ax3.plot(time_s, pwrFmu_V) 70 | ax3.set_xlabel('Time (s)') 71 | ax3.set_ylabel('Avionics Voltage (V)') 72 | ax3.set_title('Power') 73 | ax3.grid() 74 | 75 | # 3D Position 76 | fig4 = plt.figure() 77 | ax4 = fig4.gca(projection='3d', proj_type = 'ortho') 78 | ax4.plot(lonGps_deg, latGps_deg, altGps_m, '.', label='GPS') 79 | ax4.plot(lonB_deg, latB_deg, altB_m, label='Ekf') 80 | ax4.axis('equal') 81 | ax4.grid() 82 | ax4.set_xlabel('Longitude (deg)') 83 | ax4.set_ylabel('Latitude (deg)') 84 | ax4.set_title('Flight Path') 85 | ax4.legend() 86 | 87 | plt.show() 88 | 89 | return 1 90 | 91 | #%% Find Excitation Times based on 'exciteEngage' 92 | def FindExcite(oData): 93 | # returns list of tuples: 94 | #(testID, [timeMin_us, timeMax_us]) 95 | 96 | # Create array that is the testID value when exciteEngage is True and -1 everywhere else 97 | iTestExcite = np.where(oData['exciteEngage'], oData['testID'], -1 * np.ones_like(oData['testID'])) 98 | 99 | # Find where the index changes 100 | iRange = np.where(iTestExcite[:-1] != iTestExcite[1:])[0] 101 | 102 | iStartList = iRange[0::2] 103 | iEndList = iRange[1::2] 104 | 105 | excList = [] 106 | for iExc in range(0,len(iStartList)): 107 | iStart = iStartList[iExc] 108 | iEnd = iEndList[iExc] 109 | timeRange_us = [int(oData['time_us'][iStart]), int(oData['time_us'][iEnd])] 110 | 111 | testID = oData['testID'][iStart] 112 | 113 | exc = (testID, timeRange_us) 114 | excList.append(exc) 115 | 116 | return excList 117 | 118 | #%% 119 | def TestPointOut(excList, testPointList): 120 | 121 | testList = [] 122 | for iExc, exc in enumerate(excList): 123 | iTestID = exc[0] 124 | testPoint = testPointList[iTestID] 125 | testPoint['time_us'] = excList[iExc][1] 126 | testList.append(testPoint) 127 | 128 | return testList 129 | 130 | #%% Segment oData by condition 131 | import copy 132 | 133 | def SliceDict(oData, iCond): 134 | 135 | oDataSeg = {} 136 | 137 | lenCond = len(iCond) 138 | for k, v in oData.items(): 139 | if isinstance(v, dict): 140 | oDataSeg[k] = {} 141 | oDataSeg[k] = SliceDict(v, iCond) 142 | else: 143 | if v.shape[-1] >= lenCond: 144 | oDataSeg[k] = np.copy(v[...,iCond]) 145 | else: 146 | oDataSeg[k] = np.copy(v) 147 | 148 | return oDataSeg 149 | 150 | # 151 | def Segment(oData, cond): 152 | # cond = (condName, [min, max]) 153 | # if cond is a list, will return a list of oData segments 154 | # Example: cond = ('time_s', [60, 61]) 155 | 156 | # Recursive call if cond is a list of conditions 157 | if type(cond) is list: 158 | oDataSeg = [] 159 | for c in cond: 160 | seg = Segment(oData, c) 161 | oDataSeg.append(seg) 162 | return oDataSeg 163 | 164 | # Slice into Segments 165 | condName = cond[0] 166 | condRange = cond[1] 167 | if len(cond) > 2: 168 | condDesc = cond[2] 169 | else: 170 | condDesc = '' 171 | 172 | # Bool that matches the condition 173 | iCond = (oData[condName] >= condRange[0]) & (oData[condName] <= condRange[1]) 174 | 175 | # Slice, with full copy, into segmented oData. SliceDict will handle recursive calls 176 | oDataSeg = copy.deepcopy(SliceDict(oData, iCond)) 177 | oDataSeg['Desc'] = condDesc 178 | 179 | return oDataSeg 180 | 181 | # 182 | def Decimate(oData, skip): 183 | # Recursive call if cond is a list of conditions 184 | if type(skip) is list: 185 | oDataSeg = [] 186 | for s in skip: 187 | seg = Segment(oData, s) 188 | oDataSeg.append(seg) 189 | return oDataSeg 190 | 191 | # Bool that matches the condition 192 | iCond = range(0, len(oData['time_s']), skip) 193 | 194 | # Slice, with full copy, into segmented oData. SliceDict will handle recursive calls 195 | oDataSeg = copy.deepcopy(SliceDict(oData, iCond)) 196 | # oDataSeg['Desc'] = condDesc 197 | 198 | return oDataSeg 199 | -------------------------------------------------------------------------------- /Core/KinematicTransforms.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | 12 | 13 | def RelVel(vel_AO, omega_AO, pos_BA, vel_BA = np.asarray([0, 0, 0])): 14 | '''Compute the relative velocity. 15 | 16 | Inputs: 17 | vel_AO - Velocity of frame A wrt frame O 18 | omega_AO - Rotation rate of frame A wrt O 19 | pos_BA - Position of frame B wrt A 20 | vel_BA - Velocity of frame B wrt A [0, 0, 0] 21 | 22 | Outputs: 23 | vel_BO - Velocity of frame B wrt frame O 24 | 25 | Notes: 26 | There are no internal unit conversions. 27 | The vectors must all be in the same coordinate system 28 | ''' 29 | 30 | # Compute the Relative Velocity 31 | vel_BO = vel_AO + vel_BA + np.cross(omega_AO, pos_BA) 32 | 33 | return vel_BO 34 | 35 | def RelAccel(accel_AO, omega_AO, omegaDot_AO, pos_BA, accel_BA = np.asarray([0, 0, 0]), vel_BA = np.asarray([0, 0, 0])): 36 | '''Compute the relative acceleration. 37 | 38 | Inputs: 39 | accel_AO - Acceleration of frame A wrt frame O 40 | omega_AO - Rotation rate of frame A wrt O 41 | omegaDot_AO - Rotation rate of frame A wrt O 42 | pos_BA - Position of frame B wrt A 43 | accel_BA - Acceleration of frame B wrt A [0, 0, 0] 44 | vel_BA - Velocity of frame B wrt A [0, 0, 0] 45 | 46 | Outputs: 47 | accel_BO - Acceleration of frame B wrt frame O 48 | 49 | Notes: 50 | There are no internal unit conversions. 51 | The vectors must all be in the same coordinate system 52 | ''' 53 | 54 | # Compute the Relative Acceleration 55 | accel_BO = accel_AO + accel_BA + np.cross(omegaDot_AO, pos_BA) + np.cross(omega_AO, np.cross(omega_AO, pos_BA)) + 2 * np.cross(omega_AO, vel_BA) 56 | 57 | return accel_BO 58 | 59 | 60 | def D2E(r_D_, degrees = False): 61 | ''' Convert Geodetic to ECEF Coordinates 62 | 63 | Inputs: 64 | r_D_ - Position in Geodetic Coordinates (-, -, m) 65 | degrees - r_D_ is input in degrees [True] 66 | 67 | Outputs: 68 | r_E_m - Position in ECEF Coordinates (m, m, m) 69 | ''' 70 | 71 | # Change units 72 | if degrees: 73 | d2r = np.pi / 180.0 74 | r_D_ = (r_D_.T * np.asarray([d2r, d2r, 1.0])).T 75 | 76 | # Parameters for WGS84 77 | R_m = 6378137.0 # Earth semimajor axis (m) 78 | f_nd = 1/298.257223563 # reciprocal flattening (nd) 79 | 80 | # Derived parameters 81 | eSquared_nd = 2*f_nd - f_nd**2 # eccentricity squared 82 | Rew = R_m / np.sqrt(1 - eSquared_nd * np.sin(r_D_[0])**2) # Radius East-West at Latitude 83 | 84 | ## Convert 85 | r_E_m = np.nan * np.ones_like(r_D_) 86 | r_E_m[0] = (Rew + r_D_[2]) * np.cos(r_D_[0]) * np.cos(r_D_[1]) 87 | r_E_m[1] = (Rew + r_D_[2]) * np.cos(r_D_[0]) * np.sin(r_D_[1]) 88 | r_E_m[2] = (Rew * (1 - eSquared_nd) + r_D_[2]) * np.sin(r_D_[0]) 89 | 90 | return r_E_m 91 | 92 | 93 | def D2L(rRef_LD_D_, r_PD_D_, degrees = False): 94 | ''' Convert ECEF Coordinates to Local Level 95 | 96 | Inputs: 97 | rRef_LD_D_ - Reference Position in Geodetic Coordinates [-, -, m] 98 | r_PD_D_ - Position in Geodetic Coordinates 99 | 100 | Outputs: 101 | r_PL_L_m - Position in Local Level Coordinates (m) 102 | 103 | Notes: 104 | Uses D2E and E2L 105 | ''' 106 | 107 | # Change units to Radians 108 | if degrees: 109 | d2r = np.pi / 180.0 110 | rRef_LD_D_ = (rRef_LD_D_.T * np.asarray([d2r, d2r, 1.0])).T 111 | r_PD_D_ = (r_PD_D_.T * np.asarray([d2r, d2r, 1.0])).T 112 | 113 | # Reference location of L wrt D in ECEF 114 | r_LD_E_m = D2E(rRef_LD_D_, degrees = False) 115 | 116 | # Position of P wrt E in ECEF 117 | r_PE_E_m = D2E(r_PD_D_, degrees = False) 118 | 119 | lenVec = r_PD_D_.shape[-1] 120 | r_PL_E_m = np.zeros_like(r_PD_D_) 121 | for indx in range(0, lenVec): 122 | r_PL_E_m[:, indx] = r_PE_E_m[:, indx] - r_LD_E_m # Distance from Ref in ECEF 123 | 124 | T_E2L, r_PL_L_m = E2L(rRef_LD_D_, r_PL_E_m, degrees = False) 125 | 126 | return r_PL_L_m 127 | 128 | 129 | def E2L(rRef_LD_D_, r_PL_E_m, degrees = False): 130 | ''' Convert ECEF Coordinates to Local Level 131 | 132 | Inputs: 133 | rRef_LD_D_ - Reference Position in Geodetic Coordinates (ddm) 134 | r_PE_E_m - Position in ECEF Coordinates (m) 135 | 136 | Outputs: 137 | T_E2L - Transformation Matrix from ECEF to Local Level Coordinates 138 | r_L_m - Position in Local Level Coordinates (m) 139 | 140 | Notes: 141 | T_E2L = R2(270-lat)*R3(long) 142 | ''' 143 | from scipy.spatial.transform import Rotation as R 144 | 145 | # Transfor the Coordinates 146 | # Compute the Transformation Matrix at the Reference Location 147 | 148 | # Change units 149 | d2r = np.pi / 180.0 150 | if degrees: 151 | rRef_LD_D_ = (rRef_LD_D_.T * np.asarray([d2r, d2r, 1.0])).T 152 | 153 | latRef = 270.0 * d2r 154 | 155 | sRef_ = np.asarray([rRef_LD_D_[1], (latRef - rRef_LD_D_[0])]) 156 | T_E2L = R.from_euler('ZY', sRef_, degrees = False).as_dcm() # Intrinsic rotation about Z then Y 157 | 158 | # Transform Coordinates from ECEF to NED 159 | r_PL_L_m = T_E2L.T @ r_PL_E_m 160 | 161 | return T_E2L, r_PL_L_m 162 | 163 | 164 | def TransPitot(v_PA_P_mps, w_BA_B_rps, s_PB_rad, r_PB_B_m): 165 | ''' Transform Pitot Measurements from Probe location to Body. 166 | 167 | Inputs: 168 | v_PA_P_mps - Velocity of the Probe wrt Atm in Probe Coordinates [P/A]P (m/s2) 169 | w_BA_B_rps - Rotation rate of the Body Frame wrt Atm in Body Coordinates [B/A]B (rad/s) 170 | s_PB_rad - Orientation (321) of the Probe Frame wrt Body Frame [P/B] (rad) 171 | r_PB_B_m - Position of the Probe Frame wrt Body Frame in Body Coordinates [P/B]B (m) 172 | 173 | Outputs: 174 | v_BA_B_mps - Velocity of the Body wrt Atm in Body Coordinates [B/A]B (m/s2) 175 | v_BA_L_mps - Velocity of the Body wrt Atm in Local Level Coordinates [B/A]L (m/s2) 176 | ''' 177 | from scipy.spatial.transform import Rotation as R 178 | 179 | # Parameterize transformation from P to B 180 | T_B2P = R.from_euler('zyx', s_PB_rad[[2,1,0]], degrees = False).as_dcm() 181 | T_P2B = T_B2P.T 182 | 183 | v_PB_B_mps = np.asarray([0, 0, 0]) # Velocity of the Probe wrt the Body Frame [P/B]B (m/s) 184 | v_BP_B_mps = -v_PB_B_mps 185 | 186 | r_BP_B_m = -r_PB_B_m 187 | 188 | # 189 | v_BA_B_mps = np.nan * np.ones_like(v_PA_P_mps) 190 | numSamp = v_PA_P_mps.shape[-1] 191 | for indx in range(0, numSamp): 192 | # Transform from P to B 193 | v_PA_B_mps = T_P2B @ v_PA_P_mps[:,indx] 194 | v_BA_B_mps[:,indx] = RelVel(v_PA_B_mps, w_BA_B_rps[:,indx], r_BP_B_m, v_BP_B_mps) 195 | 196 | return v_BA_B_mps 197 | -------------------------------------------------------------------------------- /Examples/HuginnAirCal.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) FLT03 and FLT04 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # Constants 32 | hz2rps = 2 * np.pi 33 | rps2hz = 1 / hz2rps 34 | 35 | #%% File Lists 36 | import os.path as path 37 | 38 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 39 | 40 | fileList = {} 41 | flt = 'FLT03' 42 | fileList[flt] = {} 43 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 44 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 45 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 46 | 47 | flt = 'FLT04' 48 | fileList[flt] = {} 49 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 50 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 51 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 52 | 53 | 54 | #%% Wind/Air Cal 55 | windSegList = [ 56 | {'flt': 'FLT03', 'seg': ('time_us', [610000000, 645277887])}, 57 | {'flt': 'FLT03', 'seg': ('time_us', [722078703, 741298701])}, 58 | # {'flt': 'FLT03', 'seg': ('time_us', [893797477, 914157292])}, 59 | # 60 | # {'flt': 'FLT04', 'seg': ('time_us', [940469572, 957651007])}, 61 | # {'flt': 'FLT04', 'seg': ('time_us', [1075000000, 1112000000])}, 62 | # {'flt': 'FLT04', 'seg': ('time_us', [1112000000, 1170000000])}, 63 | ] 64 | 65 | 66 | #windSegList = [ 67 | # {'flt': 'FLT03', 'seg': ('time_us', [610000000, 741298701])}, 68 | # {'flt': 'FLT03', 'seg': ('time_us', [839997973, 925000000])}, 69 | # {'flt': 'FLT04', 'seg': ('time_us', [940469572, 957651007])} 70 | # ] 71 | 72 | oDataWindList = [] 73 | for windSeg in windSegList: 74 | fltNum = windSeg['flt'] 75 | 76 | fileLog = fileList[fltNum]['log'] 77 | fileConfig = fileList[fltNum]['config'] 78 | 79 | oData, h5Data = Loader.Log_RAPTRS(fileLog, fileConfig) 80 | oData = OpenData.Decimate(oData, 10) 81 | oDataWindList.append(OpenData.Segment(oData, windSeg['seg'])) 82 | 83 | 84 | fig, ax = plt.subplots(nrows=2) 85 | for oDataWind in oDataWindList: 86 | 87 | latGps_deg = oDataWind['rGps_D_ddm'][0] 88 | lonGps_deg = oDataWind['rGps_D_ddm'][1] 89 | latB_deg = oDataWind['rB_D_ddm'][0] 90 | lonB_deg = oDataWind['rB_D_ddm'][1] 91 | ax[0].plot(lonGps_deg, latGps_deg, '.', label='GPS') 92 | # ax[0].plot(lonB_deg, latB_deg, label='Ekf') 93 | ax[0].grid() 94 | ax[1].plot(oDataWind['time_s'], oDataWind['vIas_mps']) 95 | # ax[1].plot(oDataWind['time_s'], oDataWind['sB_L_rad'][0]*180.0/np.pi) 96 | ax[1].grid() 97 | 98 | 99 | #%% 100 | ## Pre-Optimization, Initial Guess for the Wind 101 | # Over-ride Default Error Model, Optional 102 | pData = {} 103 | pData['5Hole'] = {} 104 | pData['5Hole']['r_B_m'] = np.array([1.0, 0.0, 0.0]) 105 | pData['5Hole']['s_B_rad'] = np.array([0.0, 0.0, 0.0]) * 180.0/np.pi 106 | 107 | pData['5Hole']['v'] = {} 108 | pData['5Hole']['v']['errorType'] = 'ScaleBias+' 109 | pData['5Hole']['v']['K'] = 1.0 110 | pData['5Hole']['v']['bias'] = 0.0 111 | 112 | pData['5Hole']['alt'] = pData['5Hole']['v'].copy() 113 | pData['5Hole']['alpha'] = pData['5Hole']['v'].copy() 114 | pData['5Hole']['beta'] = pData['5Hole']['v'].copy() 115 | 116 | 117 | #%% Optimize 118 | from Core import AirData 119 | from Core import AirDataCalibration 120 | 121 | rad2deg = 180.0 / np.pi 122 | deg2rad = 1 / rad2deg 123 | 124 | oDataList = oDataWindList 125 | 126 | # Compute the optimal parameters 127 | #opt = {'Method': 'BFGS', 'Options': {'disp': True}} 128 | opt = {'Method': 'L-BFGS-B', 'Options': {'maxiter': 400, 'disp': True}} 129 | #opt = {'Method': 'CG', 'Options': {'disp': True}} 130 | 131 | #%% First Phase - Airspeed and Wind only 132 | opt['wind'] = [] 133 | for seg in oDataList: 134 | seg['vMean_AE_L_mps'] = np.asarray([0.0, 0.0, 0.0]) 135 | opt['wind'].append({'val': seg['vMean_AE_L_mps'], 'lb': np.asarray([-10, -10, -3]), 'ub': np.asarray([10, 10, 3])}) 136 | 137 | opt['param'] = [] 138 | opt['param'].append({'val': pData['5Hole']['v']['K'], 'lb': 0.80, 'ub': 1.20}) 139 | opt['param'].append({'val': pData['5Hole']['v']['bias'], 'lb': -2.0, 'ub': 2.0}) 140 | opt['param'].append({'val': pData['5Hole']['alpha']['K'], 'lb': 1.00, 'ub': 1.00}) 141 | opt['param'].append({'val': pData['5Hole']['alpha']['bias'], 'lb': -0.0 * deg2rad, 'ub': 0.0 * deg2rad}) 142 | opt['param'].append({'val': pData['5Hole']['beta']['K'], 'lb': 1.00, 'ub': 1.00}) 143 | opt['param'].append({'val': pData['5Hole']['beta']['bias'], 'lb': -0.0 * deg2rad, 'ub': 0.0 * deg2rad}) 144 | 145 | 146 | #AirDataCalibration.CostFunc(xOpt, optInfo, oDataList, param) 147 | opt['Result'] = AirDataCalibration.EstCalib(opt, oDataList, pData['5Hole']) 148 | 149 | nSegs = len(oDataWindList) 150 | nWinds = nSegs * 3 151 | vWind = opt['Result']['x'][0:nWinds].reshape((nSegs, 3)) 152 | 153 | #%% Second Phase - add alpha and beta 154 | if False: 155 | for iSeg, seg in enumerate(oDataList): 156 | seg['vMean_AE_L_mps'] = vWind[iSeg] 157 | opt['wind'][iSeg]['val'] = seg['vMean_AE_L_mps'] 158 | opt['wind'][iSeg]['lb'] = seg['vMean_AE_L_mps'] - np.asarray([0.0, 0.0, 0.0]) 159 | opt['wind'][iSeg]['ub'] = seg['vMean_AE_L_mps'] + np.asarray([0.0, 0.0, 0.0]) 160 | 161 | opt['param'][0] = {'val': pData['5Hole']['v']['K'], 'lb': pData['5Hole']['v']['K'], 'ub': pData['5Hole']['v']['K']} 162 | opt['param'][1] = {'val': pData['5Hole']['v']['bias'], 'lb': pData['5Hole']['v']['bias'], 'ub': pData['5Hole']['v']['bias']} 163 | opt['param'][2] = {'val': pData['5Hole']['alpha']['K'], 'lb': 0.75, 'ub': 1.25} 164 | opt['param'][3] = {'val': pData['5Hole']['alpha']['bias'], 'lb': -2.0 * deg2rad, 'ub': 2.0 * deg2rad} 165 | opt['param'][4] = {'val': pData['5Hole']['beta']['K'], 'lb': 0.75, 'ub': 1.25} 166 | opt['param'][5] = {'val': pData['5Hole']['beta']['bias'], 'lb': -4.0 * deg2rad, 'ub': 4.0 * deg2rad} 167 | 168 | 169 | #AirDataCalibration.CostFunc(xOpt, optInfo, oDataList, param) 170 | opt['Result'] = AirDataCalibration.EstCalib(opt, oDataList, pData['5Hole']) 171 | 172 | nSegs = len(oDataWindList) 173 | nWinds = nSegs * 3 174 | vWind = opt['Result']['x'][0:nWinds].reshape((nSegs, 3)) 175 | 176 | 177 | #%% Plot the Solution 178 | 179 | for iSeg, oDataWind in enumerate(oDataWindList): 180 | 181 | # Update the Flight data with the new calibrations 182 | calib = AirData.ApplyCalibration(oDataWind, pData['5Hole']) 183 | oDataWind.update(calib) 184 | 185 | v_BA_B_mps, v_BA_L_mps = AirData.Airspeed2NED(oDataWind['v_PA_P_mps'], oDataWind['sB_L_rad'], pData['5Hole']) 186 | 187 | oDataWind['vMean_AE_L_mps'] = vWind[iSeg] 188 | 189 | plt.figure() 190 | plt.subplot(3,1,1) 191 | plt.plot(oDataWind['time_s'], oDataWind['vB_L_mps'][0], label = 'Inertial') 192 | plt.plot(oDataWind['time_s'], v_BA_L_mps[0] + oDataWind['vMean_AE_L_mps'][0], label = 'AirData + Wind') 193 | plt.grid() 194 | plt.legend() 195 | plt.subplot(3,1,2) 196 | plt.plot(oDataWind['time_s'], oDataWind['vB_L_mps'][1]) 197 | plt.plot(oDataWind['time_s'], v_BA_L_mps[1] + oDataWind['vMean_AE_L_mps'][1]) 198 | plt.grid() 199 | plt.subplot(3,1,3) 200 | plt.plot(oDataWind['time_s'], oDataWind['vB_L_mps'][2]) 201 | plt.plot(oDataWind['time_s'], v_BA_L_mps[2] + oDataWind['vMean_AE_L_mps'][2]) 202 | plt.grid() 203 | 204 | v_AE_L_mps = np.repeat([oDataWind['vMean_AE_L_mps']], oDataWind['vB_L_mps'].shape[-1], axis=0).T 205 | vError_mps = (v_BA_L_mps + v_AE_L_mps) - oDataWind['vB_L_mps'] 206 | 207 | vErrorMag_mps = np.linalg.norm(vError_mps, axis=0) 208 | vAirMag_mps = np.linalg.norm((v_BA_L_mps + v_AE_L_mps), axis=0) 209 | vInertMag_mps = np.linalg.norm(oDataWind['vB_L_mps'], axis=0) 210 | 211 | plt.figure(0) 212 | plt.plot(vAirMag_mps, vInertMag_mps, '.') 213 | # plt.plot(vAirMag_mps, vErrorMag_mps, '.') 214 | plt.grid() 215 | 216 | 217 | print('Wind (m/s): ', oDataWind['vMean_AE_L_mps']) 218 | 219 | print('Tip Gain: ', pData['5Hole']['v']['K']) 220 | print('Tip Bias: ', pData['5Hole']['v']['bias']) 221 | print('Alpha Gain: ', pData['5Hole']['alpha']['K']) 222 | print('Alpha Bias: ', pData['5Hole']['alpha']['bias']) 223 | print('Beta Gain: ', pData['5Hole']['beta']['K']) 224 | print('Beta Bias: ', pData['5Hole']['beta']['bias']) 225 | 226 | -------------------------------------------------------------------------------- /Core/Environment.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | #import scipy.signal as signal 12 | 13 | # Constants 14 | hz2rps = 2*np.pi 15 | rps2hz = 1/hz2rps 16 | 17 | ft2m = 0.3048 18 | m2ft = 1 / ft2m 19 | 20 | 21 | 22 | #%% Generate Turbulence 23 | def TurbSpectVonKarman(sigma, L_ft, omega): 24 | ''' 25 | sigma - Turbulence Intensity [u,v,w] 26 | L_ft - Turbulence length scale [u,v,w] 27 | omega - spatial frequency (np.array) 28 | 29 | # Units of omega, dictate the units of Puvw 30 | # 31 | # omega : (rad/ft) :: Puvw : sigma_w^2 * L/pi ((ft/sec)^2 * rad/ft) 32 | # omega/(2*pi) : (cycle/ft) :: Puvw : sigma_w^2 * 2*L ((ft/sec)^2 * cycle/ft) 33 | # omega*L : (rad) :: Puvw : sigma_w^2 * 1/pi ((ft/sec)^2 * rad) 34 | # omega/V : (rad/s) :: Puvw : sigma_w^2 * L/(V*pi) ((ft/sec)^2 * rad/s) 35 | # omega*(2*pi)/V : (cycle/s) :: Puvw : sigma_w^2 * 2*L/V ((ft/sec)^2 * cycle/s) 36 | 37 | Source: MIL-HDBK-1797B 38 | MIL-HDBK-1797B is all imperial units, we want all SI units 39 | ''' 40 | # Common scale equation 41 | scale = np.asarray(sigma)**2 * (2 * np.asarray(L_ft)) / np.pi 42 | 43 | # u - direction 44 | LO2 = (1.339 * L_ft[0] * omega)**2 45 | Pu = scale[0] / (1 + LO2)**(5/6) 46 | 47 | # v - direction 48 | LO2 = (2 * 1.339 * L_ft[1] * omega)**2 49 | Pv = scale[1] * (1 + 8/3 * LO2) / (1 + LO2)**(11/6) 50 | 51 | # w - direction 52 | LO2 = (2 * 1.339 * L_ft[2] * omega)**2 53 | Pw = scale[2] * (1 + 8/3 * LO2) / (1 + LO2)**(11/6) 54 | 55 | return np.array([Pu, Pv, Pw]) 56 | 57 | 58 | def TurbSpectDryden(sigma, L_ft, omega): 59 | ''' 60 | sigma - Turbulence Intensity [u,v,w] 61 | L_ft - Turbulence length scale [u,v,w] 62 | omega - frequency (np.array) 63 | 64 | # Units of omega, dictate the units of Puvw 65 | # 66 | # omega : (rad/ft) :: Puvw : sigma_w^2 * L/pi ((ft/sec)^2 * rad/ft) 67 | # omega/(2*pi) : (cycle/ft) :: Puvw : sigma_w^2 * 2*L ((ft/sec)^2 * cycle/ft) 68 | # omega*L : (rad) :: Puvw : sigma_w^2 * 1/pi ((ft/sec)^2 * rad) 69 | # omega/V : (rad/s) :: Puvw : sigma_w^2 * L/(V*pi) ((ft/sec)^2 * rad/s) 70 | # omega*(2*pi)/V : (cycle/s) :: Puvw : sigma_w^2 * 2*L/V ((ft/sec)^2 * cycle/s) 71 | 72 | Source: MIL-HDBK-1797B 73 | MIL-HDBK-1797B is all imperial units, we want all SI units 74 | ''' 75 | # Common scale equation 76 | scale = np.asarray(sigma)**2 * (2 * np.asarray(L_ft)) / np.pi 77 | 78 | # u - direction 79 | LO2 = (L_ft[0] * omega)**2 80 | Pu = scale[0] / (1 + LO2) 81 | 82 | # v - direction 83 | LO2 = (L_ft[1] * omega)**2 84 | Pv = scale[1] * (1 + 12 * LO2) / (1 + 4 * LO2)**2 85 | 86 | # w - direction 87 | LO2 = (L_ft[2] * omega)**2 88 | Pw = scale[2] * (1 + 12 * LO2) / (1 + 4 * LO2)**2 89 | 90 | return np.array([Pu, Pv, Pw]) 91 | 92 | 93 | # Rotation Rates 94 | def TurbSpectRate(Puvw, sigma, L_ft, freq_rps, V_fps, b_ft): 95 | ''' 96 | Puvw - Turbulence Spectrum [u,v,w] ((ft/sec)^2 * rad/s) 97 | sigma - Turbulence variation [u,w,w] (ft/sec)^2 98 | L_ft - Turbulence length scale [u,v,w] (ft) 99 | freq_rps - frequency (rad/s) 100 | V_fps - Velocity (ft/s) 101 | b_ft - Wing Span (ft) 102 | ''' 103 | 104 | omega = freq_rps / V_fps 105 | 106 | # p - direction 107 | scale = sigma[2]**2 / (2 * V_fps * L_ft[2]) 108 | Pp = scale * 0.8 * (2 * np.pi * L_ft[2] / (4 * b_ft))**(1/3) / (1 + (4 * b_ft * omega / np.pi)**2) 109 | 110 | # q - direction 111 | Pq = omega**2 / (1 + (4 * b_ft * omega / np.pi)**2) * Puvw[2] 112 | 113 | # r - direction 114 | Pr = -omega**2 / (1 + (3 * b_ft * omega / np.pi)**2) * Puvw[1] 115 | 116 | 117 | return np.array([Pp, Pq, Pr]) 118 | 119 | 120 | # Turbulence Intesity 121 | def TurbIntensity(h_ft = None, U20_fps = None, level = 0): 122 | from scipy.interpolate import interp1d 123 | from scipy.interpolate import interp2d 124 | 125 | # level can be either 'light'/'common', 'moderate'/'uncommon', 'severe'/'extrordinary', or the desired probability of exceedance 126 | # Table of Probability of Exceedance is from MIL-DTL-9490E 127 | # 'severe' follows the 1e-5 curve 128 | # 'moderate' follows the 1e-3 curve 129 | # 'light' follows the 130 | 131 | kts2fps = 1.68781 132 | 133 | hLow_ft = 1000 134 | hHi_ft = 2000 135 | 136 | sigma_u = None 137 | sigma_v = None 138 | sigma_w = None 139 | 140 | if type(level) is str: 141 | if level.lower() in 'light' or 'common': 142 | probExceed = 1e-1 143 | elif level.lower() in 'moderate' or 'uncommon': 144 | probExceed = 1e-3 145 | elif level.lower() in 'severe' or 'extrordinary': 146 | probExceed = 1e-5 147 | else: 148 | probExceed = level 149 | 150 | # # 'Light' 151 | # hBrk_ft = np.array([1e3, 9e3, 18e3, 80e3]) 152 | # probExceedBrk = np.array([5, 5, 3, 3]) 153 | # 154 | # # 'Moderate' 155 | # hBrk_ft = np.array([1e3, 14e3, 45e3, 80e3]) 156 | # probExceedBrk = np.array([10, 10, 3, 3]) 157 | # 158 | # # 'Severe' 159 | # hBrk_ft = np.array([1e3, 4e3, 25e3, 80e3]) 160 | # probExceedBrk = np.array([15, 21.5, 21.5, 3]) 161 | 162 | if h_ft >= hHi_ft: 163 | 164 | hBrk_ft = np.array([500, 1750, 3750, 7500, 15e3, 25e3, 35e3, 45e3, 55e3, 65e3, 75e3, 80e3, 100e3]) 165 | probExceedBrk = np.array([2e-1, 1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) 166 | 167 | sigmaTable_fps = np.array([ 168 | [ 3.2, 2.2, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 169 | [ 4.2, 3.6, 3.3, 1.6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 170 | [ 6.6, 6.9, 7.4, 6.7, 4.6, 2.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 171 | [ 8.6, 9.6, 10.6, 10.1, 8.0, 6.6, 5.0, 4.2, 2.7, 0.0, 0.0, 0.0, 0.0], 172 | [11.8, 13.0, 16.0, 15.1, 11.6, 9.7, 8.1, 8.2, 7.9, 4.9, 3.2, 2.1, 2.1], 173 | [15.6, 17.6, 23.0, 23.6, 22.1, 20.0, 16.0, 15.1, 12.1, 7.9, 6.2, 5.1, 5.1], 174 | [18.7, 21.5, 28.4, 30.2, 30.7, 31.0, 25.2, 23.1, 17.5, 10.7, 8.4, 7.2, 7.2] 175 | ]) 176 | 177 | interpInt = interp2d(hBrk_ft, probExceedBrk, sigmaTable_fps, kind = 'linear') 178 | 179 | sigma_w = interpInt(h_ft, probExceed) 180 | 181 | if type(level) is str: 182 | sigma_w = max(sigma_w, 3) 183 | 184 | sigma_u = sigma_w 185 | sigma_v = sigma_w 186 | 187 | elif h_ft <= hLow_ft: 188 | 189 | if (U20_fps is None): 190 | if type(level) is str: 191 | if level.lower() == 'light': 192 | U20_fps = 15 * kts2fps 193 | if level.lower() == 'moderate': 194 | U20_fps = 30 * kts2fps 195 | if level.lower() == 'severe': 196 | U20_fps = 45 * kts2fps 197 | else: 198 | U20Brk = np.array([2e-1, 1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) 199 | probExceedBrk = np.array([2e-1, 1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) 200 | 201 | U20_fps = level 202 | 203 | sigma_w = 0.1 * U20_fps 204 | sigma_u = sigma_w / (0.177 + 0.000823 * h_ft)**0.4 205 | sigma_v = sigma_u 206 | 207 | if h_ft == 'terrain': 208 | 209 | probExceedBrk = np.array([2e-1, 1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) 210 | sigmaTableV_fps = np.array([4.0, 5.1, 8.0, 10.2, 12.1, 14.0, 23.1]) 211 | sigmaTableW_fps = np.array([3.5, 4.4, 7.0, 8.9, 10.5, 12.1, 17.5]) 212 | 213 | interpIntV = interp1d(probExceedBrk, sigmaTableV_fps, kind = 'linear') 214 | interpIntW = interp1d(probExceedBrk, sigmaTableW_fps, kind = 'linear') 215 | 216 | sigma_v = interpIntV(probExceed) 217 | sigma_w = interpIntW(probExceed) 218 | sigma_u = sigma_v 219 | 220 | 221 | return np.array([sigma_u, sigma_v, sigma_w]) 222 | 223 | 224 | def TurbLengthScale(h_ft = None, turbType = 'VonKarman'): 225 | 226 | hLow_ft = 1000 227 | hHi_ft = 2000 228 | 229 | if h_ft >= hHi_ft: 230 | # h_ft >= 2000 231 | # u,v,w aligned with body 232 | if turbType.lower() == 'dryden': 233 | Lu_ft = 1750 234 | Lv_ft = 0.5 * Lu_ft 235 | Lw_ft = 0.5 * Lu_ft 236 | if turbType.lower() == 'vonkarman': 237 | Lu_ft = 2500 238 | Lv_ft = 0.5 * Lu_ft 239 | Lw_ft = 0.5 * Lu_ft 240 | 241 | elif h_ft <= hLow_ft: 242 | # h_ft <= 1000 243 | # u,v,w aligned with mean wind 244 | 245 | # Ensure the height AGL is above 0 ft 246 | h_ft = max(h_ft, 0) 247 | 248 | Lu_ft = h_ft / (0.177 + 0.000823 * h_ft)**1.2 249 | Lv_ft = 0.5 * Lu_ft 250 | Lw_ft = 0.5 * h_ft 251 | 252 | else: 253 | # h_ft between 1000 and 2000 ft, Linear Interpolation 254 | LuLow_ft , LvLow_ft, LwLow_ft = TurbLengthScale(hLow_ft, turbType = turbType) 255 | LuHi_ft , LvHi_ft, LwHi_ft = TurbLengthScale(hHi_ft, turbType = turbType) 256 | 257 | hInt_nd = h_ft / (hHi_ft - hLow_ft) 258 | 259 | Lu_ft = (LuHi_ft - LuLow_ft) * hInt_nd + LuLow_ft 260 | Lv_ft = (LvHi_ft - LvLow_ft) * hInt_nd + LvLow_ft 261 | 262 | return np.array([Lu_ft , Lv_ft, Lw_ft]) 263 | 264 | 265 | # Wind Shear (MIL-DTL-9490E, 3.1.3.7.3.2) 266 | def WindShear(h_ft, u20_fps): 267 | 268 | u_fps = u20_fps * (0.46 * np.log10(h_ft) + 0.4) 269 | 270 | return u_fps 271 | -------------------------------------------------------------------------------- /Tests/TestFreqRespEstUS25e.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Example script for testing Frequency Response Estimation - US25e with Noise. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import matplotlib.patches as patch 15 | import control 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import GenExcite 28 | from Core import FreqTrans 29 | from Core import Systems 30 | 31 | # Constants 32 | hz2rps = 2*np.pi 33 | rps2hz = 1/hz2rps 34 | 35 | rad2deg = 180/np.pi 36 | deg2rad = 1/rad2deg 37 | 38 | 39 | # Load the US25e linear model 40 | exec(open("US25e_Lin.py").read()) 41 | 42 | #%% Define a linear systems 43 | freqRate_hz = 50 44 | freqRate_rps = freqRate_hz * hz2rps 45 | 46 | freqLin_hz = np.logspace(np.log10(0.01), np.log10(25), 800) 47 | freqLin_rps = freqLin_hz * hz2rps 48 | 49 | 50 | # OL : Mixer -> Plant -> SCAS_FB 51 | connectName = sysPlant.InputName[:7] + sysScas.InputName[1::3] 52 | inKeep = sysMixer.InputName + sysPlant.InputName[-7:] 53 | outKeep = sysScas.OutputName[2::4] 54 | 55 | sysOL = Systems.ConnectName([sysMixer, sysPlant, sysScas], connectName, inKeep, outKeep) 56 | 57 | 58 | # CL: Ctrl -> Plant 59 | inName = sysCtrl.InputName + sysPlant.InputName 60 | outName = sysCtrl.OutputName + sysPlant.OutputName 61 | connectName = ['cmdMotor', 'cmdElev', 'cmdRud', 'cmdAilL', 'cmdAilR', 'cmdFlapL', 'cmdFlapR', 'sensPhi', 'sensTheta', 'sensR'] 62 | inKeep = [inName[i-1] for i in [1, 2, 3, 7, 8, 9, 17, 18, 19, 20, 21, 22, 23]] 63 | outKeep = [outName[i-1] for i in [8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20]] 64 | 65 | sysCL = Systems.ConnectName([sysCtrl, sysPlant], connectName, inKeep, outKeep) 66 | 67 | # Look at only the in-out of the OL 68 | inList = [sysOL.InputName.index(s) for s in ['cmdP', 'cmdQ', 'cmdR']] 69 | outList = [sysOL.OutputName.index(s) for s in ['fbP', 'fbQ', 'fbR']] 70 | 71 | sysSimOL = sysOL[outList, :] 72 | sysSimOL = sysOL[:, inList] 73 | 74 | 75 | # Linear System Response 76 | gainLin_mag, phaseLin_rad, _ = control.freqresp(sysSimOL, omega = freqLin_rps) 77 | 78 | TxyLin = gainLin_mag * np.exp(1j*phaseLin_rad) 79 | 80 | gainLin_dB = 20 * np.log10(gainLin_mag) 81 | phaseLin_deg = np.unwrap(phaseLin_rad) * rad2deg 82 | rCritLin_mag = np.abs(TxyLin - (-1 + 0j)) 83 | 84 | sigmaLin_mag, _ = FreqTrans.Sigma(TxyLin) 85 | 86 | 87 | #%% Excitation 88 | numExc = 3 89 | numCycles = 1 90 | ampInit = 4.0 * deg2rad 91 | ampFinal = ampInit 92 | freqMinDes_rps = 0.1 * hz2rps * np.ones(numExc) 93 | freqMaxDes_rps = 10 * hz2rps * np.ones(numExc) 94 | freqStepDes_rps = (10 / freqRate_hz) * hz2rps 95 | methodSW = 'zip' # "zippered" component distribution 96 | 97 | # Generate MultiSine Frequencies 98 | freqExc_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW) 99 | freqNull_rps = freqExc_rps[0:-1] + 0.5 * np.diff(freqExc_rps) 100 | 101 | # Generate Schroeder MultiSine Signal 102 | ampExc_nd = np.linspace(ampInit, ampFinal, len(freqExc_rps)) / np.sqrt(len(freqExc_rps)) 103 | uExc, _, sigExc = GenExcite.MultiSine(freqExc_rps, ampExc_nd, sigIndx, time_s, phaseInit_rad = 0, boundPhase = 1, initZero = 1, normalize = 'peak', costType = 'Schroeder') 104 | exc_names = ['excP', 'excQ', 'excR'] 105 | 106 | # Excited Frequencies per input channel 107 | freqChan_rps = freqExc_rps[sigIndx] 108 | 109 | # Null Frequencies 110 | freqGap_rps = freqExc_rps[0:-1] + 0.5 * np.diff(freqExc_rps) 111 | 112 | # Generate Noise 113 | dist_names = ['phiDist', 'thetaDist', 'pDist', 'qDist', 'rDist', 'VDist', 'hDist'] 114 | pqrDist = np.random.normal(0, 0.25 * ampInit, size = (3, len(time_s))) 115 | angleDist = np.cumsum(pqrDist[[0,1],:], axis = -1) 116 | airDist = np.random.normal(0, 0.0, size = (2, len(time_s))) 117 | dist = np.concatenate((angleDist, pqrDist, airDist)) 118 | dist = 0.0 * dist 119 | 120 | # Reference Inputs 121 | ref_names = ['refPhi', 'refTheta', 'refYaw'] 122 | shapeRef = (len(ref_names), len(time_s)) 123 | ref = np.random.normal(0, 1.0 * ampInit, size = (3, len(time_s))) 124 | ref[1] = 2.0 * deg2rad + ref[1] 125 | ref = 0.0 * ref 126 | 127 | # Simulate the excitation through the system, with noise 128 | u = np.concatenate((ref, uExc, dist)) 129 | _, out, stateSim = control.forced_response(sysCL, T = time_s, U = u, X0 = 0.0, transpose = False) 130 | 131 | # shift output time to represent the next frame, pad t=0 with 0.0 132 | #out = np.concatenate((np.zeros((out.shape[0],1)), out[:,1:-1]), axis=1) # this is handled by a time delay on sensors in the linear simulation 133 | 134 | fbName = sysCL.OutputName[:3] 135 | fb = out[:3] 136 | 137 | vName = sysCL.OutputName[3:6] 138 | v = out[3:6] 139 | 140 | sensName = sysCL.OutputName[-7:] 141 | sens = out[-7:] 142 | 143 | #plt.plot(time_s, uExc[1], time_s, v[1], time_s, fb[1], time_s, pqrDist[1]) 144 | 145 | 146 | #%% Estimate the frequency response function 147 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.0), detrendType = 'Linear', interpType = 'linear') 148 | 149 | # Excited Frequencies per input channel 150 | optSpec.freq = freqChan_rps 151 | 152 | # FRF Estimate 153 | freq_rps, Teb, Ceb, Pee, Pbb, Peb = FreqTrans.FreqRespFuncEst(uExc, fb, optSpec) 154 | _ , Tev, Cev, _ , Pvv, Pev = FreqTrans.FreqRespFuncEst(uExc, v, optSpec) 155 | 156 | freq_hz = freq_rps * rps2hz 157 | 158 | 159 | # Form the Frequency Response, T = Teb @ Tev^-1 160 | T = np.zeros_like(Tev, dtype=complex) 161 | C = np.zeros_like(Tev, dtype=float) 162 | 163 | for i in range(T.shape[-1]): 164 | T[...,i] = (Teb[...,i].T @ np.linalg.inv(Tev[...,i].T)).T 165 | 166 | sigmaNom_mag, _ = FreqTrans.Sigma(T) # Singular Value Decomp 167 | 168 | # Coherence 169 | C = Ceb 170 | 171 | T_InputName = exc_names 172 | T_OutputName = fbName 173 | 174 | gain_mag, phase_deg = FreqTrans.GainPhase(T, magUnit='mag', phaseUnit='deg', unwrap=True) 175 | rCritNom_mag, _, _ = FreqTrans.DistCrit(T, typeUnc = 'ellipse') 176 | #rCritNom_mag, rCritUnc_mag, rCrit_mag = FreqTrans.DistCrit(T, TUnc, typeUnc = 'ellipse') 177 | #rCritNom_mag, rCritUnc_mag, rCrit_mag, pCont_mag = FreqTrans.DistCritEllipse(T, TUnc) # Returns closest approach points 178 | 179 | 180 | #%% Sigma Plot 181 | Cmin = np.min(np.min(C, axis = 0), axis = 0) 182 | sigmaNom_magMin = np.min(sigmaNom_mag, axis=0) 183 | 184 | fig = 20 185 | fig = FreqTrans.PlotSigma(freqLin_hz, sigmaLin_mag, coher_nd = np.ones_like(freqLin_hz), fig = fig, fmt = 'k', label='Linear') 186 | fig = FreqTrans.PlotSigma(freq_hz, sigmaNom_mag, coher_nd = Cmin, fmt = 'bo', fig = fig, label = 'Excitation Nominal') 187 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fmt = '--r', fig = fig, label = 'Critical Limit') 188 | ax = fig.get_axes() 189 | ax[0].set_xlim(0, 10) 190 | ax[0].set_yscale('log') 191 | ax[0].set_ylim(1e-2, 1e2) 192 | 193 | 194 | #%% Disk Margin Plots 195 | inPlot = exc_names # Elements of exc_names 196 | outPlot = fbName # Elements of fbName 197 | 198 | if False: 199 | for iOut, outName in enumerate(outPlot): 200 | for iIn, inName in enumerate(inPlot): 201 | fig = 40 + 3*iOut + iIn 202 | fig = FreqTrans.PlotSigma(freqLin_hz, rCritLin_mag[iOut, iIn], coher_nd = np.ones_like(freqLin_hz), fig = fig, fmt = 'k', label='Linear') 203 | fig = FreqTrans.PlotSigma(freq_hz[0, sigIndx[iIn]], rCritNom_mag[iOut, iIn, sigIndx[iIn]], coher_nd = C[iOut, iIn, sigIndx[iIn]], fmt = 'bo', fig = fig, label = 'Excitation') 204 | fig = FreqTrans.PlotSigma(freq_hz[0], rCritNom_mag[iOut, iIn], coher_nd = C[iOut, iIn], fmt = 'g.:', fig = fig, label = 'MIMO') 205 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fmt = '--r', fig = fig, label = 'Critical Limit') 206 | ax = fig.get_axes() 207 | ax[0].set_xlim(0, 10) 208 | ax[0].set_ylim(0, 2) 209 | fig.suptitle(inName + ' to ' + outName, size=20) 210 | 211 | #%% Nyquist Plots 212 | if False: 213 | for iOut, outName in enumerate(outPlot): 214 | for iIn, inName in enumerate(inPlot): 215 | fig = 60 + 3*iOut + iIn 216 | 217 | fig = FreqTrans.PlotNyquist(TxyLin[iOut, iIn], fig = fig, fmt = 'k', label='Linear') 218 | fig = FreqTrans.PlotNyquist(T[iOut, iIn, sigIndx[iIn]], fig = fig, fmt = 'bo', label='Excitation') 219 | fig = FreqTrans.PlotNyquist(T[iOut, iIn], fig = fig, fmt = 'g.:', label='MIMO') 220 | 221 | fig = FreqTrans.PlotNyquist(np.asarray([-1 + 0j]), TUnc = np.asarray([0.4 + 0.4j]), fig = fig, fmt = 'r+', label='Critical Region') 222 | fig.suptitle(inName + ' to ' + outName, size=20) 223 | 224 | ax = fig.get_axes() 225 | ax[0].set_xlim(-3, 1) 226 | ax[0].set_ylim(-2, 2) 227 | 228 | #%% Bode Plots 229 | if True: 230 | for iOut, outName in enumerate(outPlot): 231 | for iIn, inName in enumerate(inPlot): 232 | fig = 80 + 3*iOut + iIn 233 | fig = FreqTrans.PlotBode(freqLin_hz, gainLin_mag[iOut, iIn], phaseLin_deg[iOut, iIn], coher_nd = np.ones_like(freqLin_hz), fig = fig, fmt = 'k', label = 'Linear') 234 | fig = FreqTrans.PlotBode(freq_hz[0, sigIndx[iIn]], gain_mag[iOut, iIn, sigIndx[iIn]], phase_deg[iOut, iIn, sigIndx[iIn]], C[iOut, iIn, sigIndx[iIn]], fig = fig, fmt = 'bo', label = 'Estimated SIMO') 235 | fig = FreqTrans.PlotBode(freq_hz[0], gain_mag[iOut, iIn], phase_deg[iOut, iIn], C[iOut, iIn], fig = fig, fmt = 'g.', label = 'Estimated MIMO') 236 | fig.suptitle(inName + ' to ' + outName, size=20) 237 | 238 | -------------------------------------------------------------------------------- /Examples/HuginnAirCal2.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) FLT03 and FLT04 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # Constants 32 | hz2rps = 2 * np.pi 33 | rps2hz = 1 / hz2rps 34 | 35 | #%% File Lists 36 | import os.path as path 37 | 38 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 39 | #pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Huginn') 40 | #pathBase = path.join('D:/', 'Huginn') 41 | 42 | fileList = {} 43 | flt = 'FLT05' 44 | fileList[flt] = {} 45 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 46 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 47 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 48 | 49 | flt = 'FLT06' 50 | fileList[flt] = {} 51 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 52 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 53 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 54 | 55 | 56 | #%% Wind/Air Cal 57 | windSegList = [ 58 | {'flt': 'FLT05', 'seg': ('time_us', [566483686, 582408497])}, 59 | {'flt': 'FLT05', 'seg': ('time_us', [602534178, 622279236])}, 60 | {'flt': 'FLT05', 'seg': ('time_us', [637362791, 654286351])}, 61 | {'flt': 'FLT05', 'seg': ('time_us', [666668777, 687832534])}, 62 | {'flt': 'FLT05', 'seg': ('time_us', [703115100, 766364351])}, # Long!! 63 | {'flt': 'FLT05', 'seg': ('time_us', [788467105, 799488311])}, 64 | {'flt': 'FLT05', 'seg': ('time_us', [811669552, 831211361])}, 65 | {'flt': 'FLT05', 'seg': ('time_us', [844412511, 861513899])}, 66 | {'flt': 'FLT05', 'seg': ('time_us', [873694795, 887575754])}, 67 | {'flt': 'FLT05', 'seg': ('time_us', [899096534, 909897237])}, 68 | {'flt': 'FLT05', 'seg': ('time_us', [927000000, 950000000])}, # Landing Approach 69 | {'flt': 'FLT06', 'seg': ('time_us', [940358346, 955822061])}, 70 | {'flt': 'FLT06', 'seg': ('time_us', [982747328, 1000069848])}, 71 | {'flt': 'FLT06', 'seg': ('time_us', [1010491142, 1026492809])}, 72 | {'flt': 'FLT06', 'seg': ('time_us', [1036733749, 1054855133])}, 73 | {'flt': 'FLT06', 'seg': ('time_us', [1065295790, 1087597269])}, # Slowing Turn 74 | {'flt': 'FLT06', 'seg': ('time_us', [1103958408, 1122539650])}, 75 | {'flt': 'FLT06', 'seg': ('time_us', [1140000000, 1165401057])}, 76 | {'flt': 'FLT06', 'seg': ('time_us', [1165401057, 1189143263])}, 77 | {'flt': 'FLT06', 'seg': ('time_us', [1189143263, 1225000000])}, # Landing Approach 78 | {'flt': 'FLT06', 'seg': ('time_us', [1225000000, 1260000000])}, # Landing Approach 79 | 80 | 81 | ] 82 | 83 | 84 | oDataWindList = [] 85 | for windSeg in windSegList: 86 | fltNum = windSeg['flt'] 87 | 88 | fileLog = fileList[fltNum]['log'] 89 | fileConfig = fileList[fltNum]['config'] 90 | 91 | oData, h5Data = Loader.Log_RAPTRS(fileLog, fileConfig) 92 | 93 | for key in h5Data['Sensor-Processing']['PostProcess']['INS'].keys(): 94 | oData[key] = h5Data['Sensor-Processing']['PostProcess']['INS'][key] 95 | 96 | oData = OpenData.Decimate(oData, 10) 97 | oDataWindList.append(OpenData.Segment(oData, windSeg['seg'])) 98 | 99 | 100 | fig, ax = plt.subplots(nrows=2) 101 | for oDataWind in oDataWindList: 102 | 103 | latGps_deg = oDataWind['rGps_D_ddm'][0] 104 | lonGps_deg = oDataWind['rGps_D_ddm'][1] 105 | latB_deg = oDataWind['rB_D_ddm'][0] 106 | lonB_deg = oDataWind['rB_D_ddm'][1] 107 | ax[0].plot(lonGps_deg, latGps_deg, '.', label='GPS') 108 | ax[0].plot(lonB_deg, latB_deg, label='Ekf') 109 | ax[0].grid() 110 | ax[1].plot(oDataWind['time_s'], oDataWind['vIas_mps']) 111 | ax[1].plot(oDataWind['time_s'], oDataWind['sB_L_rad'][0]*180.0/np.pi) 112 | ax[1].grid() 113 | 114 | 115 | #%% 116 | ## Pre-Optimization, Initial Guess for the Wind 117 | # Over-ride Default Error Model, Optional 118 | pData = {} 119 | pData['5Hole'] = {} 120 | pData['5Hole']['r_B_m'] = np.array([1.0, 0.0, 0.0]) 121 | pData['5Hole']['s_B_rad'] = np.array([0.0, 0.0, 0.0]) * 180.0/np.pi 122 | 123 | pData['5Hole']['v'] = {} 124 | pData['5Hole']['v']['errorType'] = 'ScaleBias+' 125 | pData['5Hole']['v']['K'] = 1.0 126 | pData['5Hole']['v']['bias'] = 0.0 127 | 128 | pData['5Hole']['alt'] = pData['5Hole']['v'].copy() 129 | pData['5Hole']['alpha'] = pData['5Hole']['v'].copy() 130 | pData['5Hole']['beta'] = pData['5Hole']['v'].copy() 131 | 132 | pData['5Hole']['v']['K'] = 0.95 133 | 134 | #%% Optimize 135 | from Core import AirData 136 | from Core import AirDataCalibration 137 | 138 | rad2deg = 180.0 / np.pi 139 | deg2rad = 1 / rad2deg 140 | 141 | oDataList = oDataWindList 142 | 143 | # Compute the optimal parameters 144 | #opt = {'Method': 'BFGS', 'Options': {'disp': True}} 145 | opt = {'Method': 'L-BFGS-B', 'Options': {'disp': True}} 146 | #opt = {'Method': 'L-BFGS-B', 'Options': {'maxiter': 10, 'disp': True}} 147 | #opt = {'Method': 'CG', 'Options': {'disp': True}} 148 | 149 | #%% First Phase - Airspeed and Wind only 150 | opt['wind'] = [] 151 | for seg in oDataList: 152 | seg['vMean_AE_L_mps'] = np.asarray([-2.0, 0.0, 0.0]) 153 | opt['wind'].append({'val': seg['vMean_AE_L_mps'], 'lb': np.asarray([-10, -10, -3]), 'ub': np.asarray([10, 10, 3])}) 154 | 155 | opt['param'] = [] 156 | opt['param'].append({'val': pData['5Hole']['v']['K'], 'lb': 0.80, 'ub': 1.20}) 157 | opt['param'].append({'val': pData['5Hole']['v']['bias'], 'lb': -3.0, 'ub': 3.0}) 158 | opt['param'].append({'val': pData['5Hole']['alpha']['K'], 'lb': 1.00, 'ub': 1.00}) 159 | opt['param'].append({'val': pData['5Hole']['alpha']['bias'], 'lb': -0.0 * deg2rad, 'ub': 0.0 * deg2rad}) 160 | opt['param'].append({'val': pData['5Hole']['beta']['K'], 'lb': 1.00, 'ub': 1.00}) 161 | opt['param'].append({'val': pData['5Hole']['beta']['bias'], 'lb': -0.0 * deg2rad, 'ub': 0.0 * deg2rad}) 162 | 163 | 164 | #AirDataCalibration.CostFunc(xOpt, optInfo, oDataList, param) 165 | opt['Result'] = AirDataCalibration.EstCalib(opt, oDataList, pData['5Hole']) 166 | 167 | nSegs = len(oDataWindList) 168 | nWinds = nSegs * 3 169 | vWind = opt['Result']['x'][0:nWinds].reshape((nSegs, 3)) 170 | 171 | #%% Second Phase - add alpha and beta 172 | if False: 173 | for iSeg, seg in enumerate(oDataList): 174 | seg['vMean_AE_L_mps'] = vWind[iSeg] 175 | opt['wind'][iSeg]['val'] = seg['vMean_AE_L_mps'] 176 | opt['wind'][iSeg]['lb'] = seg['vMean_AE_L_mps'] - np.asarray([0.0, 0.0, 0.0]) 177 | opt['wind'][iSeg]['ub'] = seg['vMean_AE_L_mps'] + np.asarray([0.0, 0.0, 0.0]) 178 | 179 | opt['param'][0] = {'val': pData['5Hole']['v']['K'], 'lb': 1.0 * pData['5Hole']['v']['K'], 'ub': 1.0 * pData['5Hole']['v']['K']} 180 | opt['param'][1] = {'val': pData['5Hole']['v']['bias'], 'lb': pData['5Hole']['v']['bias']-0.0, 'ub': pData['5Hole']['v']['bias']+0.0} 181 | opt['param'][2] = {'val': pData['5Hole']['alpha']['K'], 'lb': 0.8, 'ub': 1.2} 182 | opt['param'][3] = {'val': pData['5Hole']['alpha']['bias'], 'lb': -4.0 * deg2rad, 'ub': 4.0 * deg2rad} 183 | opt['param'][4] = {'val': pData['5Hole']['beta']['K'], 'lb': 0.8, 'ub': 1.2} 184 | opt['param'][5] = {'val': pData['5Hole']['beta']['bias'], 'lb': -4.0 * deg2rad, 'ub': 4.0 * deg2rad} 185 | 186 | 187 | #AirDataCalibration.CostFunc(xOpt, optInfo, oDataList, param) 188 | opt['Result'] = AirDataCalibration.EstCalib(opt, oDataList, pData['5Hole']) 189 | 190 | nSegs = len(oDataWindList) 191 | nWinds = nSegs * 3 192 | vWind = opt['Result']['x'][0:nWinds].reshape((nSegs, 3)) 193 | 194 | 195 | #%% Plot the Solution 196 | from SensorModel import SensorErrorModel 197 | 198 | for iSeg, oDataWind in enumerate(oDataWindList): 199 | 200 | # Update the Flight data with the new calibrations 201 | calib = AirData.ApplyCalibration(oDataWind, pData['5Hole']) 202 | oDataWind.update(calib) 203 | 204 | v_BA_B_mps, v_BA_L_mps = AirData.Airspeed2NED(oDataWind['v_PA_P_mps'], oDataWind['sB_L_rad'], pData['5Hole']) 205 | 206 | oDataWind['vMean_AE_L_mps'] = vWind[iSeg] 207 | 208 | if True: 209 | plt.figure() 210 | plt.subplot(3,1,1) 211 | plt.plot(oDataWind['time_s'], oDataWind['vB_L_mps'][0], label = 'Inertial') 212 | plt.plot(oDataWind['time_s'], v_BA_L_mps[0] + oDataWind['vMean_AE_L_mps'][0], label = 'AirData + Wind') 213 | plt.grid() 214 | plt.legend() 215 | plt.subplot(3,1,2) 216 | plt.plot(oDataWind['time_s'], oDataWind['vB_L_mps'][1]) 217 | plt.plot(oDataWind['time_s'], v_BA_L_mps[1] + oDataWind['vMean_AE_L_mps'][1]) 218 | plt.grid() 219 | plt.subplot(3,1,3) 220 | plt.plot(oDataWind['time_s'], oDataWind['vB_L_mps'][2]) 221 | plt.plot(oDataWind['time_s'], v_BA_L_mps[2] + oDataWind['vMean_AE_L_mps'][2]) 222 | plt.grid() 223 | 224 | v_AE_L_mps = np.repeat([oDataWind['vMean_AE_L_mps']], oDataWind['vB_L_mps'].shape[-1], axis=0).T 225 | vError_mps = (v_BA_L_mps + v_AE_L_mps) - oDataWind['vB_L_mps'] 226 | 227 | vErrorMag_mps = np.linalg.norm(vError_mps, axis=0) 228 | vAirUncalMag_mps = oDataWind['vIas_mps'] 229 | vAirMag_mps = np.linalg.norm((v_BA_L_mps + v_AE_L_mps), axis=0) 230 | vInertMag_mps = np.linalg.norm(oDataWind['vB_L_mps'], axis=0) 231 | 232 | plt.figure(0) 233 | # plt.plot(vAirMag_mps, vInertMag_mps - vAirMag_mps, '.') 234 | plt.plot(vAirMag_mps, vInertMag_mps, '.') 235 | plt.grid() 236 | 237 | print('Wind (m/s): ', oDataWind['vMean_AE_L_mps']) 238 | 239 | 240 | plt.figure(0) 241 | vA_mps = np.linspace(15, 35, 5) 242 | vAcal_mps = SensorErrorModel(vA_mps, pData['5Hole']['v']) 243 | plt.plot(vA_mps, vA_mps, 'k:') 244 | #plt.plot(vA_mps, vA_mps - vG_mps, 'k:') 245 | 246 | print('Velocity Gain: ', pData['5Hole']['v']['K']) 247 | print('Velocity Bias: ', pData['5Hole']['v']['bias']) 248 | print('Alpha Gain: ', pData['5Hole']['alpha']['K']) 249 | print('Alpha Bias: ', pData['5Hole']['alpha']['bias']) 250 | print('Beta Gain: ', pData['5Hole']['beta']['K']) 251 | print('Beta Bias: ', pData['5Hole']['beta']['bias']) 252 | 253 | -------------------------------------------------------------------------------- /Examples/HuginnRtsm.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) FLT 03-06 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # Constants 32 | hz2rps = 2 * np.pi 33 | rps2hz = 1 / hz2rps 34 | 35 | 36 | #%% File Lists 37 | import os.path as path 38 | 39 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 40 | #pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Huginn') 41 | #pathBase = path.join('D:/', 'Huginn') 42 | 43 | fileList = {} 44 | flt = 'FLT03' 45 | fileList[flt] = {} 46 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 47 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 48 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 49 | 50 | flt = 'FLT04' 51 | fileList[flt] = {} 52 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 53 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 54 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 55 | 56 | flt = 'FLT05' 57 | fileList[flt] = {} 58 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 59 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 60 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 61 | 62 | flt = 'FLT06' 63 | fileList[flt] = {} 64 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 65 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 66 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 67 | 68 | 69 | #%% 70 | from Core import FreqTrans 71 | 72 | rtsmSegList = [ 73 | {'flt': 'FLT03', 'seg': ('time_us', [693458513 , 705458513], 'FLT03 - 23 m/s'), 'fmt': 'k'}, # 23 m/s 74 | {'flt': 'FLT03', 'seg': ('time_us', [865877709 , 877877709], 'FLT03 - 20 m/s'), 'fmt': 'k'}, # 20 m/s 75 | 76 | {'flt': 'FLT04', 'seg': ('time_us', [884683583, 896683583], 'FLT04 - 26 m/s'), 'fmt': 'k'}, # 26 m/s 77 | {'flt': 'FLT04', 'seg': ('time_us', [998733748, 1010733748], 'FLT04 - 20 m/s'), 'fmt': 'k'}, # 20 m/s 78 | 79 | {'flt': 'FLT06', 'seg': ('time_us', [1122539650, 1134539650], 'FLT06 - 23 m/s'), 'fmt': 'k'}, # 23 m/s 80 | 81 | {'flt': 'FLT05', 'seg': ('time_us', [582408497, 594408497], 'FLT05 - 26 m/s'), 'fmt': 'b'}, # 26 m/s 82 | {'flt': 'FLT05', 'seg': ('time_us', [799488311, 811488311], 'FLT05 - 29 m/s'), 'fmt': 'g'}, # 29 m/s 83 | 84 | {'flt': 'FLT06', 'seg': ('time_us', [955822061, 967822061], 'FLT06 - 32 m/s'), 'fmt': 'm'}, # 32 m/s 85 | ] 86 | 87 | 88 | oDataSegs = [] 89 | for rtsmSeg in rtsmSegList: 90 | fltNum = rtsmSeg['flt'] 91 | 92 | fileLog = fileList[fltNum]['log'] 93 | fileConfig = fileList[fltNum]['config'] 94 | 95 | # Load 96 | h5Data = Loader.Load_h5(fileLog) # RAPTRS log data as hdf5 97 | sysConfig = Loader.JsonRead(fileConfig) 98 | oData = Loader.OpenData_RAPTRS(h5Data, sysConfig) 99 | 100 | # Create Signal for Bending Measurement 101 | aZ = np.array([ 102 | oData['aCenterFwdIMU_IMU_mps2'][2] - oData['aCenterFwdIMU_IMU_mps2'][2][0], 103 | oData['aCenterAftIMU_IMU_mps2'][2] - oData['aCenterAftIMU_IMU_mps2'][2][0], 104 | oData['aLeftMidIMU_IMU_mps2'][2] - oData['aLeftMidIMU_IMU_mps2'][2][0], 105 | oData['aLeftFwdIMU_IMU_mps2'][2] - oData['aLeftFwdIMU_IMU_mps2'][2][0], 106 | oData['aLeftAftIMU_IMU_mps2'][2] - oData['aLeftAftIMU_IMU_mps2'][2][0], 107 | oData['aRightMidIMU_IMU_mps2'][2] - oData['aRightMidIMU_IMU_mps2'][2][0], 108 | oData['aRightFwdIMU_IMU_mps2'][2] - oData['aRightFwdIMU_IMU_mps2'][2][0], 109 | oData['aRightAftIMU_IMU_mps2'][2] - oData['aRightAftIMU_IMU_mps2'][2][0] 110 | ]) 111 | 112 | aCoefEta1 = np.array([456.1, -565.1, -289.9, 605.4, 472.4, -292, 605.6, 472.2]) * 0.3048 113 | measEta1 = aCoefEta1 @ (aZ - np.mean(aZ, axis = 0)) * 1/50 * 1e-3 114 | 115 | 116 | aCoefEta1dt = np.array([2.956, -2.998, -1.141, 5.974, 5.349, -1.149, 5.974, 5.348]) * 0.3048 117 | measEta1dt = aCoefEta1dt @ (aZ - np.mean(aZ, axis = 0)) * 1e-3 118 | 119 | # Added signals 120 | oData['cmdRoll_FF'] = h5Data['Control']['cmdRoll_PID_rpsFF'] 121 | oData['cmdRoll_FB'] = h5Data['Control']['cmdRoll_PID_rpsFB'] 122 | oData['cmdPitch_FF'] = h5Data['Control']['cmdPitch_PID_rpsFF'] 123 | oData['cmdPitch_FB'] = h5Data['Control']['cmdPitch_PID_rpsFB'] 124 | oData['cmdBend_FF'] = h5Data['Control']['refBend_nd'] # h5Data['Excitation']['Bend']['cmdBend_nd'] 125 | oData['cmdBendDt_FB'] = measEta1dt 126 | oData['cmdBend_FB'] = measEta1 127 | 128 | # Segments 129 | seg = OpenData.Segment(oData, rtsmSeg['seg']) 130 | oDataSegs.append(seg) 131 | 132 | # plt.plot(seg['time_s'], seg['Control']['cmdBend_nd'], seg['time_s'], seg['cmdBendDt_FB'], seg['time_s'], seg['cmdBend_FB']) 133 | 134 | 135 | #%% 136 | 137 | sigExcList = ['cmdRoll_rps', 'cmdPitch_rps', 'cmdBend_nd'] 138 | sigFbList = ['cmdRoll_FB', 'cmdPitch_FB', 'cmdBend_FB'] 139 | sigFfList = ['cmdRoll_FF', 'cmdPitch_FF', 'cmdBend_FF'] 140 | 141 | freqExc_rps = [] 142 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_1']['Frequency'])) 143 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_2']['Frequency'])) 144 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_3']['Frequency'])) 145 | 146 | vCmdList = [] 147 | vExcList = [] 148 | vFbList = [] 149 | vFfList = [] 150 | for iSeg, seg in enumerate(oDataSegs): 151 | vCmd = np.zeros((len(sigExcList), len(seg['time_s']))) 152 | vExc = np.zeros((len(sigExcList), len(seg['time_s']))) 153 | vFb = np.zeros((len(sigExcList), len(seg['time_s']))) 154 | vFf = np.zeros((len(sigExcList), len(seg['time_s']))) 155 | 156 | for iSig, sigExc in enumerate(sigExcList): 157 | sigFb = sigFbList[iSig] 158 | sigFf = sigFfList[iSig] 159 | 160 | vCmd[iSig] = seg['Control'][sigExc] 161 | vExc[iSig] = seg['Excitation'][sigExc] 162 | vFb[iSig][1:-1] = seg[sigFb][0:-2] # Shift the time of the output into next frame 163 | vFf[iSig] = seg[sigFf] 164 | 165 | vCmdList.append(vCmd) 166 | vExcList.append(vExc) 167 | vFbList.append(vFb) 168 | vFfList.append(vFf) 169 | 170 | plt.plot(oDataSegs[iSeg]['time_s'], oDataSegs[iSeg]['vIas_mps']) 171 | 172 | # plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][0]) 173 | # plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][1]) 174 | # plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][2]) 175 | # plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][0]) 176 | # plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][1]) 177 | # plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][2]) 178 | 179 | 180 | #%% Estimate the frequency response function 181 | # Define the excitation frequencies 182 | freqRate_hz = 50 183 | freqRate_rps = freqRate_hz * hz2rps 184 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.2), detrendType = 'Linear') 185 | 186 | # Excited Frequencies per input channel 187 | optSpec.freq = np.asarray(freqExc_rps) 188 | 189 | # FRF Estimate 190 | T = [] 191 | C = [] 192 | for iSeg, seg in enumerate(oDataSegs): 193 | 194 | freq_rps, Teb, Ceb, Pee, Pbb, Peb = FreqTrans.FreqRespFuncEst(vExcList[iSeg], vFbList[iSeg], optSpec) 195 | _ , Tev, Cev, _ , Pvv, Pev = FreqTrans.FreqRespFuncEst(vExcList[iSeg], vCmdList[iSeg], optSpec) 196 | 197 | freq_hz = freq_rps * rps2hz 198 | 199 | # Form the Frequency Response 200 | T_seg = np.empty_like(Tev) 201 | 202 | for i in range(T_seg.shape[-1]): 203 | T_seg[...,i] = Teb[...,i] @ np.linalg.inv(Tev[...,i]) 204 | 205 | 206 | T.append( T_seg ) 207 | 208 | # C.append(Cev) 209 | C.append(Ceb) 210 | 211 | 212 | T_InputNames = sigExcList 213 | T_OutputNames = sigFbList 214 | 215 | # Compute Gain, Phase, Crit Distance 216 | gain_mag = [] 217 | phase_deg = [] 218 | rCritNom_mag = [] 219 | rCrit_mag = [] 220 | sNom = [] 221 | for iSeg in range(0, len(oDataSegs)): 222 | 223 | gainElem_mag, _, _ = FreqTrans.DistCritCirc(T[iSeg], pCrit = 0+0j, typeNorm = 'RSS') 224 | 225 | gain_mag.append(gainElem_mag) 226 | 227 | phase_deg.append(FreqTrans.Phase(T[iSeg], phaseUnit = 'deg', unwrap = True)) 228 | 229 | nom_mag, _, _= FreqTrans.DistCritCirc(T[iSeg], pCrit = -1+0j, typeNorm = 'RSS') 230 | 231 | rCritNom_mag.append(nom_mag) 232 | 233 | sNom_seg, _ = FreqTrans.Sigma(T[iSeg]) # Singular Value Decomp, U @ S @ Vh == T[...,i] 234 | sNom.append(sNom_seg) 235 | 236 | 237 | #%% Sigma Plot 238 | fig = None 239 | for iSeg in range(0, len(oDataSegs)): 240 | Cmin = np.min(np.min(C[iSeg], axis = 0), axis = 0) 241 | sNomMin = np.min(sNom[iSeg], axis=0) 242 | 243 | fig = FreqTrans.PlotSigma(freq_hz[0], sNomMin, coher_nd = Cmin, fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 244 | 245 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fmt = '--r', fig = fig) 246 | 247 | ax = fig.get_axes() 248 | ax[0].set_xlim(0, 10) 249 | ax[0].set_ylim(0, 1) 250 | 251 | 252 | #%% Disk Margin Plots 253 | inPlot = sigExcList # Elements of sigExcList 254 | outPlot = sigFbList # Elements of sigFbList 255 | 256 | if False: 257 | #%% 258 | for iOut, outName in enumerate(outPlot): 259 | for iIn, inName in enumerate(inPlot): 260 | 261 | fig = None 262 | for iSeg in range(0, len(oDataSegs)): 263 | fig = FreqTrans.PlotSigma(freq_hz[0], rCritNom_mag[iSeg][iOut, iIn], coher_nd = C[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 264 | 265 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fig = fig, fmt = 'r--', label = 'Critical Limit') 266 | fig.suptitle(inName + ' to ' + outName, size=20) 267 | 268 | ax = fig.get_axes() 269 | ax[0].set_ylim(0, 2) 270 | 271 | 272 | #%% Nyquist Plots 273 | if False: 274 | #%% 275 | for iOut, outName in enumerate(outPlot): 276 | for iIn, inName in enumerate(inPlot): 277 | 278 | fig = None 279 | for iSeg in range(0, len(oDataSegs)): 280 | fig = FreqTrans.PlotNyquist(T[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*', label = oDataSegs[iSeg]['Desc']) 281 | 282 | fig = FreqTrans.PlotNyquist(np.asarray([-1+ 0j]), TUnc = np.asarray([0.4 + 0.4j]), fig = fig, fmt = '*r', label = 'Critical Region') 283 | fig.suptitle(inName + ' to ' + outName, size=20) 284 | 285 | ax = fig.get_axes() 286 | ax[0].set_xlim(-3, 1) 287 | ax[0].set_ylim(-2, 2) 288 | 289 | 290 | #%% Bode Plots 291 | if False: 292 | #%% 293 | for iOut, outName in enumerate(outPlot): 294 | for iIn, inName in enumerate(inPlot): 295 | 296 | fig = None 297 | for iSeg in range(0, len(oDataSegs)): 298 | fig = FreqTrans.PlotBode(freq_hz[0], gain_mag[iSeg][iOut, iIn], phase_deg[iSeg][iOut, iIn], C[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 299 | 300 | fig.suptitle(inName + ' to ' + outName, size=20) 301 | -------------------------------------------------------------------------------- /Core/AirData.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | 12 | from SensorModel import SensorErrorModel 13 | import KinematicTransforms 14 | 15 | # Constants 16 | vSoundSL_mps = 340.29 # standard sea level speed of sound, m/s 17 | pStdSL_Pa = 101325.0 # standard sea level pressure, Pa 18 | tempStdSL_K = 288.15 # standard sea level temperature, K 19 | 20 | L = 0.0065 # standard lapse rate, K/m 21 | R = 8.314 # gas constant, J/kg-mol 22 | M = 0.02895 # molecular mass dry air, kg/mol 23 | g = 9.807 # acceleration due to gravity at sea level, m/s/s 24 | 25 | #%% Compute Indicated airspeed 26 | def IAS(pTip_Pa): 27 | 28 | temp = 5.0 * (((pTip_Pa / pStdSL_Pa + 1.0) ** (2.0/7.0)) - 1.0) 29 | # vIas_mps = vSoundSL_mps * np.sign(temp) * np.sqrt(np.sign(temp) * np.abs(temp)) 30 | vIas_mps = vSoundSL_mps * np.sign(temp) * np.sqrt(np.abs(temp)) 31 | 32 | return vIas_mps 33 | 34 | #%% Compute Equivalent airspeed 35 | def EAS(pTip_Pa, pStatic_Pa): 36 | 37 | temp = 5.0 * pStatic_Pa / pStdSL_Pa * (((pTip_Pa / pStatic_Pa + 1.0) ** (2.0/7.0)) - 1.0) 38 | # vEas_mps = vSoundSL_mps * np.sign(temp) * np.sqrt(np.sign(temp) * np.abs(temp)) 39 | vEas_mps = vSoundSL_mps * np.sign(temp) * np.sqrt(np.abs(temp)) 40 | 41 | return vEas_mps 42 | 43 | #%% Compute True airspeed 44 | def TAS(pTip_Pa, pStatic_Pa, temp_C): 45 | 46 | vEas_mps = EAS(pTip_Pa, pStatic_Pa) 47 | vTas_mps = vEas_mps * np.sqrt((temp_C + 273.15) / tempStdSL_K) 48 | 49 | return vTas_mps 50 | 51 | #%% Compute pressure altitude 52 | def PressAltitude(pStatic_Pa): 53 | 54 | altPress_m = (tempStdSL_K / L) * (1.0 - ((pStatic_Pa / pStdSL_Pa) ** ((L * R)/(M * g)))) 55 | 56 | return altPress_m 57 | 58 | #%% Computes density altitude 59 | def DensAltitude(pStatic_Pa, temp_C): 60 | 61 | altDens_m = (tempStdSL_K / L) * (1.0 - (((pStatic_Pa / pStdSL_Pa) * (tempStdSL_K/(temp_C+273.15))) ** ((L*R)/(M*g - L*R)))); 62 | 63 | return altDens_m 64 | 65 | #%% Computes atmospheric density 66 | def AtmDensity(pStatic_Pa, temp_C): 67 | 68 | rho_kgpm3 = (M * pStatic_Pa) / (R * (temp_C + 273.15)) 69 | 70 | return rho_kgpm3 71 | 72 | 73 | #%% Angle of Attack or Sidslip estimate when all ports are measuring pressure difference from static ring 74 | # For alpha: Angle ports are alpha, Side ports are beta 75 | # For beta: Angle ports are beta, Side ports are alpha 76 | def AeroAngle1(pTip, pAngle1, pAngle2, pSide1, pSide2, kCal): 77 | 78 | pNorm = pTip - 0.5 * (pSide1 + pSide2) 79 | 80 | angle = (pAngle1 - pAngle2) / (kCal * pNorm) 81 | 82 | return angle 83 | 84 | #%% Angle of Attack estimate when pAlpha is measuring pAlpha1-pAlpha2 directly 85 | def AeroAngle2(pTip, pAngle, kCal): 86 | 87 | angle = pAngle / (kCal * pTip); 88 | 89 | return angle 90 | 91 | 92 | #%% 93 | def Airspeed2NED(v_PA_P_mps, s_BL_rad, param): 94 | 95 | from scipy.spatial.transform import Rotation as R 96 | 97 | # Assume the rotation rate of the atmosphere is negligible 98 | w_AL_L_rps = np.zeros_like(v_PA_P_mps) 99 | 100 | # Compute the Rotation rate of the Probe wrt the Atm 101 | # w_BA_B_rps = w_BL_B_rps + T_L2B * w_AL_L_rps 102 | # w_BA_B_rps = w_BL_B_rps + w_AL_L_rps # FIXIT - should have transformation from L to B 103 | w_BA_B_rps = np.zeros_like(v_PA_P_mps) 104 | 105 | # Translate and Rotate the Pitot measurement to the Body frame 106 | v_BA_B_mps = KinematicTransforms.TransPitot(v_PA_P_mps, w_BA_B_rps, param['s_B_rad'], param['r_B_m']); 107 | 108 | # Transform Coordinates from B to L 109 | v_BA_L_mps = np.zeros_like(v_BA_B_mps) 110 | numSamp = v_PA_P_mps.shape[-1] 111 | for iSamp in range(0, numSamp): 112 | # T_B2L = R.from_euler('XYZ', -s_BL_rad[:,iSamp], degrees = False).as_dcm().T 113 | T_B2L = R.from_euler('ZYX', s_BL_rad[[2,1,0], iSamp], degrees = False).as_dcm() 114 | 115 | # Compute the NED velocity 116 | v_BA_L_mps[:,iSamp] = T_B2L @ v_BA_B_mps[:,iSamp] 117 | 118 | return v_BA_B_mps, v_BA_L_mps 119 | 120 | 121 | #%% Airdata 122 | def ApplyPressureCalibration(meas, param): 123 | ''' Compute the Airdata Parameters with a given sensor error/calibration model. 124 | 125 | Inputs: 126 | meas - Dictionary of measured airdata sensor values 127 | (pTip_Pa) - Magnitude of the Differential Pressure of the Probe wrt Atm [P/A] (Pa) 128 | (pStatic_Pa) - Magnitude of the Static Pressure of the Probe wrt Atm [P/A] (Pa) 129 | (tempProbe_C)- Temperature of the Airdata transducers (C) 130 | (pAlpha_Pa) - Optional, Magnitude of the Alpha1 Pressure of the Probe wrt Atm [P/A] (Pa) 131 | (pAlpha1_Pa) - Optional, Magnitude of the Alpha1 Pressure of the Probe wrt Atm [P/A] (Pa) 132 | (pAlpha2_Pa) - Optional, Magnitude of the Alpha1 Pressure of the Probe wrt Atm [P/A] (Pa) 133 | (pBeta_Pa) - Optional, Magnitude of the Alpha1 Pressure of the Probe wrt Atm [P/A] (Pa) 134 | (pBeta1_Pa) - Optional, Magnitude of the Alpha1 Pressure of the Probe wrt Atm [P/A] (Pa) 135 | (pBeta2_Pa) - Optional, Magnitude of the Alpha1 Pressure of the Probe wrt Atm [P/A] (Pa) 136 | 137 | param - Dictionary of airdata calibration parameters 138 | (r_B_m) - Position of the Probe Frame wrt Body Frame in Body Coordinates [P/B]B (m) 139 | (s_B_rad) - Orientation ('ZYX') of the Probe Frame wrt Body Frame [P/B] (rad) 140 | (pTip) - Dictionary of individual sensor error model parameters 141 | (pStatic) - Dictionary of individual sensor error model parameters 142 | (pXX) - Dictionary of individual sensor error model parameters, matches keys in 'meas' 143 | 144 | (airpseed) - Dictionary of parameters to compute airspeed from pressures 145 | (alt) - Dictionary of parameters to compute altitude from pressures 146 | (alpha) - Dictionary of parameters to compute alpha from pressures 147 | (beta) - Dictionary of parameters to compute alpha from pressures 148 | 149 | Outputs: 150 | calib - Dictionary of airdata parameters and calibrated sensor values 151 | (vIas_mps) 152 | (VEas_mps) 153 | (VTas_mps) 154 | (altPres_m) 155 | (alpha_rad) 156 | (beta_rad) 157 | (v_PA_P_mps) - Velocity of the Probe wrt Atm in Probe Coordinates (aka: u,v,w) [P/A]P (m/s2) 158 | 159 | ''' 160 | 161 | # Constants 162 | r2d = 180.0 / np.pi 163 | 164 | ## Apply Error Models, Estimate the "true" measures from the Error Models 165 | # Apply Pitot-Static Error Models 166 | calib = {} 167 | for key, val in meas.items(): 168 | if key in ['pTip_Pa', 'pStatic_Pa', 'tempProbe_C', 'pAlpha_Pa', 'pAlpha1_Pa', 'pAlpha2_Pa', 'pBeta_Pa', 'pBeta1_Pa', 'pBeta2_Pa']: 169 | pKey = key.split('_')[0] # The param key should be just the units stripped off 170 | if pKey in param.keys(): 171 | calib[key] = SensorErrorModel(val, param[pKey]) 172 | else: # Just copy the data 173 | calib[key] = val 174 | 175 | 176 | # Determine Probe type 177 | typeProbe = None 178 | if 'pAlpha_Pa' in calib.keys(): 179 | typeProbe = '5Hole2' 180 | elif 'pAlpha1_Pa' in calib.keys(): 181 | typeProbe = '5Hole1' 182 | else: 183 | typeProbe = 'Pitot' 184 | 185 | # 186 | numSamp = calib['pTip_Pa'].shape[-1] 187 | calib['v_PA_P_mps'] = np.zeros((3, numSamp)) 188 | 189 | if typeProbe in ['Pitot']: 190 | # Airspeeds 191 | calib['vIas_mps'] = IAS(calib['pTip_Pa']) 192 | calib['vEas_mps'] = EAS(calib['pTip_Pa'], calib['pStatic_Pa']) 193 | calib['vTas_mps'] = TAS(calib['pTip_Pa'], calib['pStatic_Pa'], calib['tempProbe_C']) 194 | 195 | # Pressure Altitude 196 | calib['altPress_m'] = PressAltitude(calib['pStatic_Pa']) 197 | 198 | # Airspeed Vector ['u', 'v', 'w'] 199 | calib['v_PA_P_mps'][0] = calib['vIas_mps'] 200 | 201 | if typeProbe in ['5Hole1']: 202 | # Airspeeds 203 | calib['vIas_mps'] = IAS(calib['pTip_Pa']) 204 | calib['vEas_mps'] = EAS(calib['pTip_Pa'], calib['pStatic_Pa']) 205 | calib['vTas_mps'] = TAS(calib['pTip_Pa'], calib['pStatic_Pa'], calib['tempProbe_C']) 206 | 207 | # Pressure Altitude 208 | calib['altPress_m'] = PressAltitude(calib['pStatic_Pa']) 209 | 210 | # Inflow Angles: Angle of Attack and Sideslip 211 | calib['alpha_rad'] = AeroAngle1(calib['pTip_Pa'], calib['pAlpha1_Pa'], calib['pAlpha2_Pa'], calib['pBeta1_Pa'], calib['pBeta2_Pa'], param['alphaCal']) 212 | calib['alpha_deg'] = calib['alpha_rad'] * r2d 213 | 214 | calib['beta_rad'] = AeroAngle1(calib['pTip_Pa'], calib['pBeta1_Pa'], calib['pBeta2_Pa'], calib['pAlpha1_Pa'], calib['pAlpha2_Pa'], param['betaCal']) 215 | calib['beta_deg'] = calib['beta_rad'] * r2d 216 | 217 | # Airspeed Vector ['u', 'v', 'w'] 218 | calib['v_PA_P_mps'][0] = calib['vIas_mps'] 219 | # calib['v_PA_P_mps'][0] = calib['vIas_mps'] / (np.cos(calib['alpha_rad']) * np.cos(calib['beta_rad'])) 220 | calib['v_PA_P_mps'][1] = calib['vIas_mps'] * np.sin(calib['beta_rad']) 221 | calib['v_PA_P_mps'][2] = calib['v_PA_P_mps'][0] * np.tan(calib['alpha_rad']) 222 | 223 | if typeProbe in ['5Hole2']: 224 | # Airspeeds 225 | calib['vIas_mps'] = IAS(calib['pTip_Pa']) 226 | calib['vEas_mps'] = EAS(calib['pTip_Pa'], calib['pStatic_Pa']) 227 | calib['vTas_mps'] = TAS(calib['pTip_Pa'], calib['pStatic_Pa'], calib['tempProbe_C']) 228 | 229 | # Pressure Altitude 230 | calib['altPress_m'] = PressAltitude(calib['pStatic_Pa']) 231 | 232 | # Inflow Angles: Angle of Attack and Sideslip 233 | calib['alpha_rad'] = AeroAngle2(calib['pTip_Pa'], calib['pAlpha_Pa'], param['alphaCal']) 234 | calib['alpha_deg'] = calib['alpha_rad'] * r2d 235 | 236 | calib['beta_rad'] = AeroAngle2(calib['pTip_Pa'], calib['pBeta_Pa'], param['betaCal']) 237 | calib['beta_deg'] = calib['beta_rad'] * r2d 238 | 239 | # Airspeed Vector ['u', 'v', 'w'] 240 | calib['v_PA_P_mps'][0] = calib['vIas_mps'] 241 | # calib['v_PA_P_mps'][0] = calib['vIas_mps'] / (np.cos(calib['alpha_rad']) * np.cos(calib['beta_rad'])) 242 | calib['v_PA_P_mps'][1] = calib['vIas_mps'] * np.sin(calib['beta_rad']) 243 | calib['v_PA_P_mps'][2] = calib['v_PA_P_mps'][0] * np.tan(calib['alpha_rad']) 244 | 245 | 246 | return calib 247 | 248 | #%% Airdata 249 | def ApplyCalibration(meas, param): 250 | ''' Compute the Airdata Parameters with a given sensor error/calibration model. 251 | 252 | Inputs: 253 | meas - Dictionary of measured airdata sensor values 254 | (vIas_mps) - 255 | (altPress_m) - 256 | (alpha_rad) - 257 | (beta_rad) - 258 | 259 | param - Dictionary of airdata calibration parameters 260 | (v) - 261 | 262 | Outputs: 263 | calib - Dictionary of airdata parameters and calibrated sensor values 264 | (vCal_mps) 265 | (altCal_m) 266 | (alphaCal_rad) 267 | (betaCal_rad) 268 | (v_PA_P_mps) - Velocity of the Probe wrt Atm in Probe Coordinates (aka: u,v,w) [P/A]P (m/s2) 269 | 270 | ''' 271 | 272 | # Constants 273 | r2d = 180.0 / np.pi 274 | 275 | ## Apply Error Models 276 | calib = {} 277 | 278 | # Airspeeds 279 | calib['vCal_mps'] = SensorErrorModel(meas['vIas_mps'], param['v']) 280 | 281 | # Pressure Altitude 282 | if ('altBaro_m' in meas) and ('alt' in param): 283 | calib['altCal_m'] = SensorErrorModel(meas['altBaro_m'], param['alt']) 284 | else: 285 | calib['altCal_m'] = np.zeros_like(meas['vIas_mps']) 286 | 287 | # Inflow Angle: Angle of Attack 288 | if ('alpha_rad' in meas) and ('alpha' in param): 289 | calib['alphaCal_rad'] = SensorErrorModel(meas['alpha_rad'], param['alpha']) 290 | else: 291 | calib['alphaCal_rad'] = np.zeros_like(meas['vIas_mps']) 292 | 293 | calib['alphaCal_deg'] = calib['alphaCal_rad'] * r2d 294 | 295 | # Inflow Angle: Angle of Sideslip 296 | if ('beta_rad' in meas) and ('beta' in param): 297 | calib['betaCal_rad'] = SensorErrorModel(meas['beta_rad'], param['beta']) 298 | else: 299 | calib['betaCal_rad'] = np.zeros_like(meas['vIas_mps']) 300 | 301 | calib['betaCal_deg'] = calib['betaCal_rad'] * r2d 302 | 303 | 304 | # Airspeed Vector ['u', 'v', 'w'] 305 | calib['v_PA_P_mps'] = np.zeros((3, calib['vCal_mps'].shape[-1])) 306 | 307 | calib['v_PA_P_mps'][0] = calib['vCal_mps'] 308 | # calib['v_PA_P_mps'][0] = calib['vCal_mps'] / (np.cos(calib['alphaCal_rad']) * np.cos(calib['betaCal_rad'])) 309 | calib['v_PA_P_mps'][1] = calib['vCal_mps'] * np.sin(calib['betaCal_rad']) 310 | calib['v_PA_P_mps'][2] = calib['v_PA_P_mps'][0] * np.tan(calib['alphaCal_rad']) 311 | 312 | 313 | return calib 314 | -------------------------------------------------------------------------------- /Tests/TestMultisineSpacing.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Example script for generating Schroeder type Multisine Excitations. 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # Hack to allow loading the Core package 16 | if __name__ == "__main__" and __package__ is None: 17 | from sys import path, argv 18 | from os.path import dirname, abspath, join 19 | 20 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 21 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 22 | 23 | del path, argv, dirname, abspath, join 24 | 25 | from Core import GenExcite 26 | from Core import FreqTrans 27 | 28 | 29 | # Constants 30 | pi = np.pi 31 | hz2rps = 2*pi 32 | rps2hz = 1/hz2rps 33 | 34 | 35 | #%% 36 | numCycles = 3 37 | freqMin_hz = 0.1; freqMin_rps = freqMin_hz * hz2rps 38 | freqRate_hz = 50; freqRate_rps = freqRate_hz * hz2rps 39 | 40 | T = numCycles / freqMin_hz 41 | dt = 1 / freqRate_hz 42 | N = T / dt = T * freqRate_hz = numCycles * freqRate_hz / freqMin_hz 43 | 44 | #theta: [-pi, pi] -> thetaStep_rad = 2 * pi / N 45 | #FFT: [-N/2, N/2] -> freqStep_rps = freqRate_rps / N = 2*pi * freqRate_hz / N = 2*pi * freqMin_hz / numCycles 46 | # freqMax_rps = freqRate_rps / 2 47 | # freqMin_rps = -freqRate_rps / 2 48 | # freqStep_rps = (freqMax_rps - freqMin_rps) / N = freqRate_rps / N 49 | # A "bin" is (freqRate_rps / N) wide : bin = freqRate_rps / N 50 | # freq_rps = 2*pi * k / (N * dt), for k = 0, 1, 2, ...., N-1 (Harris1980) 51 | # freqStep_rps = 2*pi / (N * dt) 52 | 53 | # For narrow band DFT or CZT 54 | # "bin" is retained: bin = 55 | #freqStep_hz is defined: freqStep_hz = (freqMax_hz - freqMin_hz) / M 56 | # bin = freqRate_rps / N 57 | 58 | #%% Signal length - Window analytic 59 | numSampList = np.arange(100, 1001, 100) 60 | numSampMin = np.min(numSampList) 61 | numSampMax = np.max(numSampList) 62 | 63 | theta = np.linspace(0, 0.05*np.pi, numSampMin) 64 | theta = theta[1:] 65 | bins = theta * numSampMin / (2*np.pi) 66 | 67 | W_theshold_dB = -18 68 | 69 | plt.figure(1) 70 | binList = [] 71 | for i, N in enumerate(numSampList): 72 | W_mag = np.exp(-1j * theta * (N-1)/2) * np.sin(N * theta/2) / np.sin(theta/2) # Rectangular Window 73 | W_mag = np.abs(W_mag) / np.max(np.abs(W_mag)) 74 | 75 | W_dB = 20 * np.log10(W_mag) 76 | plt.plot(bins, W_dB) 77 | 78 | bin_theshold = 2 * np.interp(-W_theshold_dB, -W_dB, bins) 79 | plt.plot(bin_theshold/2, W_theshold_dB, '*') 80 | 81 | 82 | binList.append( bin_theshold) 83 | 84 | plt.grid(True) 85 | plt.xlabel('Normalized Bin') 86 | plt.ylabel('Normalized Power (dB)') 87 | plt.ylim(bottom = -60, top = 10) 88 | plt.show 89 | 90 | # 91 | plt.figure(2) 92 | #overSamp = numSampList / numSampMin 93 | plt.loglog(np.asarray(binList), numSampList, '-*', label = str(W_theshold_dB) + 'dB Bandwidth') 94 | plt.xlabel('Number of Samples') 95 | plt.ylabel('Number of bins') 96 | plt.legend() 97 | plt.grid(True) 98 | plt.show 99 | 100 | #%% Signal length - Window only 101 | numSampList = np.arange(100, 1001, 100) 102 | numSampMax = int(np.max(numSampList)) 103 | numSampMin = int(np.min(numSampList)) 104 | overSamp = numSampList / numSampMin 105 | 106 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = 1 * hz2rps, smooth = ('box', 1)) 107 | #optSpec.scaleType = 'density' 108 | optSpec.winType = ('tukey', 0.0) 109 | optSpec.freq = np.linspace(0, 0.05*np.pi, numSampMin-1) 110 | 111 | sig = np.ones(numSampMax) 112 | noiseSamples = np.random.randn(numSampMax) 113 | noiseLev = 0.0 114 | sig += noiseLev * noiseSamples 115 | 116 | P_theshold_dB = -12 117 | plt.figure(3) 118 | binList = [] 119 | for numSamp in numSampList: 120 | numSamp = int(numSamp) 121 | 122 | theta, sigDft, P_mag = FreqTrans.Spectrum(sig[0:numSamp], optSpec) 123 | P_mag = np.abs(P_mag / np.max(P_mag)) 124 | P_dB = 20*np.log10(P_mag[0]) 125 | 126 | bins = theta[0] * len(optSpec.freq[0]) / (2*np.pi) 127 | plt.plot(bins, P_dB, '-', label = 'Window: ' + str(optSpec.winType) + ' Over Sample: ' + str(numSamp/numSampMin)) 128 | 129 | bin_theshold = 2 * np.interp(-P_theshold_dB, -P_dB, bins) 130 | plt.plot(bin_theshold/2, P_theshold_dB, '*') 131 | 132 | binList.append( bin_theshold ) 133 | 134 | plt.xlabel('Normalized Bin') 135 | plt.ylabel('Normalized Power (dB)') 136 | plt.ylim(bottom = -60, top = 10) 137 | plt.xlim(left = 0.0) 138 | plt.grid(); plt.legend() 139 | plt.show 140 | 141 | # 142 | plt.figure(4) 143 | plt.loglog(numSampList, np.asarray(binList), '-*', label = str(P_theshold_dB) + 'dB Threshold') 144 | plt.xlabel('Number of Samples') 145 | plt.ylabel('Number of bins') 146 | plt.grid(True) 147 | plt.legend() 148 | plt.show 149 | 150 | 151 | #%% Window effects - Noise and Windows 152 | numSamp = 500 153 | sig = np.ones(numSamp) 154 | noiseSamples = np.random.randn(numSamp) 155 | 156 | numBin = 500 157 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = 1 * hz2rps, smooth = ('box', 1)) 158 | optSpec.freq = np.linspace(0, 0.05*np.pi, numBin-1) 159 | 160 | noiseList = np.linspace(0.0, 1.0, 5) 161 | winList = [('tukey', 0.0), ('tukey', 0.1), ('tukey', 0.5), 'hann', 'hamming', 'blackman', 'blackmanharris'] 162 | #winList = [('tukey', 0.0), 'blackmanharris'] 163 | 164 | if True: 165 | for win in winList: 166 | plt.figure(5) 167 | for noiseLev in noiseList: 168 | noise = sig + noiseLev * noiseSamples 169 | 170 | optSpec.winType = win 171 | theta, _, P_mag = FreqTrans.Spectrum(noise, optSpec) 172 | P_mag = np.abs(P_mag / np.max(P_mag)) 173 | P_dB = 20*np.log10(P_mag[0]) 174 | 175 | bins = theta[0] * len(optSpec.freq[0]) / (2*np.pi) 176 | plt.plot(bins, P_dB, '-', label = 'Window: ' + str(optSpec.winType) + ' - Noise Mag: ' + str(noiseLev)) 177 | 178 | plt.xlabel('Bins (-)') 179 | plt.ylabel('Power Spectrum (dB)') 180 | plt.grid(); plt.legend() 181 | plt.show 182 | 183 | if True: 184 | for noiseLev in noiseList: 185 | noise = sig + noiseLev * noiseSamples 186 | 187 | plt.figure(6) 188 | for win in winList: 189 | optSpec.winType = win 190 | theta, _, P_mag = FreqTrans.Spectrum(noise, optSpec) 191 | P_mag = np.abs(P_mag / np.max(P_mag)) 192 | P_dB = 20*np.log10(P_mag[0]) 193 | 194 | bins = theta[0] * len(optSpec.freq[0]) / (2*np.pi) 195 | plt.plot(bins, P_dB, '-', label = 'Window: ' + str(optSpec.winType) + ' - Noise Mag: ' + str(noiseLev)) 196 | 197 | plt.xlabel('Bins (-)') 198 | plt.ylabel('Power Spectrum (dB)') 199 | plt.grid(); plt.legend() 200 | plt.show 201 | 202 | 203 | #%% Signal Length - Oscillating Signals 204 | freqRate_hz = 100 205 | freq_rps = np.array([1, 2]) * hz2rps 206 | numCycles = np.array([1, 2, 3, 4, 5, 10]) 207 | numCycleMin = np.min(numCycles) 208 | numCycleMax = np.max(numCycles) 209 | 210 | timeDur_s = numCycleMax/(np.min(freq_rps) * rps2hz) 211 | time_s = np.linspace(0, timeDur_s, int(timeDur_s*freqRate_hz) + 1) 212 | sig = 1 * np.sin(freq_rps[0] * time_s) + 1 * np.sin(freq_rps[1] * time_s) 213 | 214 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, smooth = ('box', 1)) 215 | 216 | #winList = [('tukey', 0.0), ('tukey', 0.1), 'hann', 'hamming', 'blackman'] 217 | optSpec.winType = ('tukey', 0.0) 218 | numFreq = int((len(sig) - 1) * (numCycleMin/numCycleMax)) 219 | optSpec.freq = np.linspace(np.min(freq_rps)-0.5*hz2rps, np.max(freq_rps)+0.5*hz2rps, numFreq) 220 | 221 | plt.figure(7) 222 | for iCycles in numCycles: 223 | iMax = int((len(sig) - 1) * (iCycles/numCycleMax) + 1) 224 | 225 | freq_rps, _, P_mag = FreqTrans.Spectrum(sig[0:iMax], optSpec) 226 | freq_hz = freq_rps * rps2hz 227 | P_dB = 20*np.log10(P_mag) 228 | 229 | plt.plot(freq_hz[0], P_mag[0], '-', label = 'Cycles: ' + str(iCycles)) 230 | 231 | #plt.xlim(1, 2) 232 | plt.xlabel('Frequency (Hz)') 233 | plt.grid(True) 234 | #plt.ylim(bottom = -60, top = 10) 235 | plt.legend() 236 | plt.show 237 | 238 | 239 | #%% Define the frequency selection and distribution of the frequencies into the signals 240 | numChan = 2 241 | freqRate_hz = 50; 242 | timeDur_s = 10.0 243 | numCycles = 1 244 | 245 | freqMinDes_rps = (numCycles/timeDur_s) * hz2rps * np.ones(numChan) 246 | #freqMaxDes_rps = (freqRate_hz/2) * hz2rps * np.ones(numChan) 247 | freqMaxDes_rps = 10 * hz2rps * np.ones(numChan) 248 | freqStepDes_rps = (40 / freqRate_hz) * hz2rps 249 | methodSW = 'zip' # "zippered" component distribution 250 | 251 | ## Generate MultiSine Frequencies 252 | freqElem_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_rps, numCycles, freqStepDes_rps, methodSW) 253 | timeDur_s = time_s[-1] - time_s[0] 254 | 255 | ## Generate Schroeder MultiSine Signal 256 | ampElem_nd = np.ones_like(freqElem_rps) ## Approximate relative signal amplitude, create flat 257 | sigList, phaseElem_rad, sigElem = GenExcite.MultiSine(freqElem_rps, ampElem_nd, sigIndx, time_s, costType = 'Schroeder', phaseInit_rad = 0, boundPhase = True, initZero = True, normalize = 'peak'); 258 | 259 | 260 | ## Results 261 | peakFactor = GenExcite.PeakFactor(sigList) 262 | peakFactorRel = peakFactor / np.sqrt(2) 263 | excStd = np.std(sigList, axis = -1) 264 | print(peakFactorRel) 265 | 266 | # Signal Power 267 | sigPowerRel = (ampElem_nd / max(ampElem_nd))**2 / len(ampElem_nd) 268 | 269 | if True: 270 | fig, ax = plt.subplots(ncols=1, nrows=1, sharex=True) 271 | for iChan in range(0, numChan): 272 | plt.plot(time_s, sigList[iChan]) 273 | plt.ylabel('Amplitude (nd)') 274 | plt.grid(True) 275 | plt.xlabel('Time (s)') 276 | 277 | 278 | #%% Plot the Excitation Spectrum 279 | sig = sigList[0] 280 | 281 | round(len(sig) / len(freqElem_rps)) 282 | 283 | dFreq_rps = np.mean(np.diff(freqElem_rps)) 284 | freqNull_rps = freqElem_rps[0:-1] + 0.5 * dFreq_rps 285 | freqDense_rps = np.arange(freqElem_rps[0], freqElem_rps[-1], dFreq_rps/36) 286 | 287 | winType = ('tukey', 0.0) 288 | #winType = 'bartlett' 289 | smooth = ('box', 1) 290 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, freq = freqElem_rps, smooth = smooth, winType = winType) 291 | optSpecN = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, freq = freqNull_rps, smooth = smooth, winType = winType) 292 | optSpecD = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, freq = freqDense_rps, smooth = smooth, winType = winType) 293 | 294 | 295 | freq_rps, _, P_mag = FreqTrans.Spectrum(sig, optSpec) 296 | freq_hz = freq_rps * rps2hz 297 | P_dB = 20*np.log10(P_mag) 298 | 299 | freqN_rps, _, Pnull_mag = FreqTrans.Spectrum(sig, optSpecN) 300 | freqN_hz = freqN_rps * rps2hz 301 | Pnull_dB = 20*np.log10(Pnull_mag) 302 | 303 | freqD_rps, _, Pdens_mag = FreqTrans.Spectrum(sig, optSpecD) 304 | freqD_hz = freqD_rps * rps2hz 305 | Pdens_dB = 20*np.log10(Pdens_mag) 306 | 307 | 308 | plt.figure(8) 309 | plt.plot(freqD_hz[0], Pdens_dB[0], ':', label = 'Dense Fill Set') 310 | plt.plot(freq_hz[0], P_dB[0], '*', label = 'Excited Set') 311 | plt.plot(freqN_hz[0], Pnull_dB[0], '*', label = 'Null Set') 312 | plt.legend() 313 | plt.grid() 314 | 315 | 316 | #%% This shows that 317 | from scipy import signal 318 | x = np.ones(2**9) 319 | #x = sig 320 | N = len(x) 321 | 322 | freqRate_hz = 70 323 | freqRate_rps = freqRate_hz * hz2rps 324 | 325 | rps2bin = N / freqRate_rps 326 | bin2rps = 1/rps2bin 327 | 328 | M = 60 329 | fBin_rps = 1 * bin2rps # widh of a bin in rps 330 | fBin_hz = 1 * bin2rps * rps2hz 331 | freqCZT_hz = np.linspace(0.0, 4 * fBin_hz, M) 332 | #freqCZT_hz = np.arange(0.0, (M) * fBin_hz, fBin_hz) 333 | freqCZT_rps = freqCZT_hz * hz2rps 334 | 335 | freqCZT2_hz = freqCZT_hz + fBin_hz 336 | freqCZT2_rps = freqCZT2_hz * hz2rps 337 | 338 | winType = ('tukey', 0.0) 339 | win = signal.get_window(winType, N) 340 | optSpecFFT = FreqTrans.OptSpect(dftType = 'fft', scaleType = 'density', freqRate = freqRate_hz * hz2rps, smooth = smooth, winType = winType) 341 | optSpecCZT = FreqTrans.OptSpect(dftType = 'czt', scaleType = 'density', freqRate = freqRate_hz * hz2rps, freq = freqCZT_rps, smooth = smooth, winType = winType) 342 | optSpecCZT2 = FreqTrans.OptSpect(dftType = 'czt', scaleType = 'density', freqRate = freqRate_hz * hz2rps, freq = freqCZT2_rps, smooth = smooth, winType = winType) 343 | 344 | freqFFT_rps, _, P_FFT_mag = FreqTrans.Spectrum(x, optSpecFFT) 345 | freqFFT_hz = freqFFT_rps * rps2hz 346 | P_FFT_dB = 20*np.log10(P_FFT_mag) 347 | 348 | freqCZT_rps, _, P_CZT_mag = FreqTrans.Spectrum(x, optSpecCZT) 349 | freqCZT_hz = freqCZT_rps * rps2hz 350 | P_CZT_dB = 20*np.log10(P_CZT_mag) 351 | 352 | freqCZT2_rps, _, P_CZT2_mag = FreqTrans.Spectrum(x, optSpecCZT2) 353 | freqCZT2_hz = freqCZT2_rps * rps2hz 354 | P_CZT2_dB = 20*np.log10(P_CZT2_mag) 355 | 356 | #plt.figure() 357 | #plt.plot(freqFFT_hz[0], P_FFT_mag[0] / P_FFT_mag[0].max(), '-k') 358 | #plt.plot(freqCZT_hz[0], P_CZT_mag[0] / P_CZT_mag[0].max(), '-b') 359 | 360 | plt.figure(11) 361 | #freqStep_rps = freqRate_rps / N/2 = 2*pi * freqRate_hz / N/2 = 2*pi * freqMin_hz / numCycles 362 | 363 | plt.plot(freqFFT_rps[0] * 1, P_FFT_mag[0] / P_FFT_mag[0].max(), '-k') 364 | plt.plot(freqCZT_rps[0] * 1, P_CZT_mag[0] / P_CZT_mag[0].max(), ':.b') 365 | plt.plot(freqCZT2_rps[0] * 1, P_CZT2_mag[0] / P_CZT_mag[0].max(), '.g') 366 | plt.xlim([0,3]) 367 | -------------------------------------------------------------------------------- /Examples/ThorRtsm.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Thor RTSM 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # plt.rcParams.update({ 32 | # "text.usetex": True, 33 | # "font.family": "serif", 34 | # "font.serif": ["Palatino"], 35 | # "font.size": 10 36 | # }) 37 | 38 | # Constants 39 | hz2rps = 2 * np.pi 40 | rps2hz = 1 / hz2rps 41 | 42 | 43 | #%% File Lists 44 | import os.path as path 45 | 46 | # pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Thor') 47 | pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Thor') 48 | 49 | fileList = {} 50 | flt = 'FLT126' 51 | fileList[flt] = {} 52 | fileList[flt]['log'] = path.join(pathBase, 'Thor' + flt, 'Thor' + flt + '.h5') 53 | fileList[flt]['config'] = path.join(pathBase, 'Thor' + flt, 'thor.json') 54 | fileList[flt]['def'] = path.join(pathBase, 'Thor' + flt, 'thor_def.json') 55 | 56 | flt = 'FLT127' 57 | fileList[flt] = {} 58 | fileList[flt]['log'] = path.join(pathBase, 'Thor' + flt, 'Thor' + flt + '.h5') 59 | fileList[flt]['config'] = path.join(pathBase, 'Thor' + flt, 'thor.json') 60 | fileList[flt]['def'] = path.join(pathBase, 'Thor' + flt, 'thor_def.json') 61 | 62 | flt = 'FLT128' 63 | fileList[flt] = {} 64 | fileList[flt]['log'] = path.join(pathBase, 'Thor' + flt, 'Thor' + flt + '.h5') 65 | fileList[flt]['config'] = path.join(pathBase, 'Thor' + flt, 'thor.json') 66 | fileList[flt]['def'] = path.join(pathBase, 'Thor' + flt, 'thor_def.json') 67 | 68 | 69 | #%% 70 | from Core import FreqTrans 71 | 72 | rtsmSegList = [ 73 | # {'flt': 'FLT126', 'seg': ('time_us', [875171956 , 887171956], 'FLT126 - RTSM - Nominal Gain, 4 deg amp'), 'color': 'k'}, 74 | # {'flt': 'FLT126', 'seg': ('time_us', [829130591 , 841130591], 'FLT126 - RTSM Route - Nominal Gain, 4 deg amp'), 'color': 'k'}, 75 | # {'flt': 'FLT127', 'seg': ('time_us', [641655909 , 653655909], 'FLT127 - RTSM Route - Nominal Gain, 4 deg amp'), 'color': 'k'}, # Yaw controller in-op?? 76 | # {'flt': 'FLT128', 'seg': ('time_us', [700263746 , 712263746 ], 'FLT128 - RTSM Route - Nominal Gain, 4 deg amp'), 'color': 'k'}, # Interesting Roll Margin vs. Uncertainty 77 | # {'flt': 'FLT128', 'seg': ('time_us', [831753831 , 843753831 ], 'FLT128 - RTSM Route - Nominal Gain, 4 deg amp'), 'color': 'k'}, 78 | # {'flt': 'FLT128', 'seg': ('time_us', [ 959859721 , 971859721 ], 'FLT128 - RTSM Route - Nominal Gain, 4 deg amp'), 'color': 'k'}, # Not good 79 | 80 | # {'flt': 'FLT126', 'seg': ('time_us', [928833763 , 940833763], 'FLT126 - RTSM Large - Nominal Gain, 8 deg amp'), 'color': 'r'}, 81 | # {'flt': 'FLT127', 'seg': ('time_us', [698755386 , 707255278], 'FLT127 - RTSM Large Route - Nominal Gain, 8 deg amp'), 'color': 'r'}, # Yaw controller in-op?? 82 | # {'flt': 'FLT128', 'seg': ('time_us', [779830919 , 791830919 ], 'FLT128 - RTSM Large Route - Nominal Gain, 8 deg amp'), 'color': 'r'}, 83 | # {'flt': 'FLT128', 'seg': ('time_us', [900237086 , 912237086 ], 'FLT128 - RTSM Large Route - Nominal Gain, 8 deg amp'), 'color': 'r'}, 84 | # 85 | # {'flt': 'FLT126', 'seg': ('time_us', [902952886 , 924952886], 'FLT126 - RTSM Long - Nominal Gain, 4 deg amp'), 'color': 'b'}, 86 | # {'flt': 'FLT127', 'seg': ('time_us', [657015836 , 689015836], 'FLT127 - RTSM Long Route - Nominal Gain, 4 deg amp'), 'color': 'b'}, # Yaw controller in-op?? 87 | # {'flt': 'FLT128', 'seg': ('time_us', [714385469 , 746385469 ], 'FLT128 - RTSM Long Route - Nominal Gain, 4 deg amp'), 'color': 'b'}, 88 | {'flt': 'FLT128', 'seg': ('time_us', [847254621 , 879254621 ], 'FLT128 - RTSM Long Route - Nominal Gain, 4 deg amp'), 'color': 'b'}, # Best 89 | 90 | # {'flt': 'FLT127', 'seg': ('time_us', [1209355236 , 1221535868], 'FLT127 - RTSM LongLarge Route - Nominal Gain, 8 deg amp'), 'color': 'm'}, # Yaw controller in-op?? 91 | # {'flt': 'FLT128', 'seg': ('time_us', [794251787 , 826251787 ], 'FLT128 - RTSM LongLarge Route - Nominal Gain, 8 deg amp'), 'color': 'm'}, 92 | # {'flt': 'FLT128', 'seg': ('time_us', [921438015 , 953438015 ], 'FLT128 - RTSM LongLarge Route - Nominal Gain, 8 deg amp'), 'color': 'm'}, 93 | 94 | # {'flt': 'FLT126', 'seg': ('time_us', [981115495 , 993115495], 'FLT126 - RTSM - High Gain, 4 deg amp')}, 95 | # {'flt': 'FLT126', 'seg': ('time_us', [689907125 , 711907125], 'FLT126 - RTSM Long - High Gain, 4 deg amp')}, 96 | # {'flt': 'FLT126', 'seg': ('time_us', [728048050 , 740048050], 'FLT126 - RTSM Large - High Gain, 8 deg amp')}, 97 | # 98 | ] 99 | 100 | 101 | oDataSegs = [] 102 | for rtsmSeg in rtsmSegList: 103 | fltNum = rtsmSeg['flt'] 104 | 105 | fileLog = fileList[fltNum]['log'] 106 | fileConfig = fileList[fltNum]['config'] 107 | 108 | # Load 109 | h5Data = Loader.Load_h5(fileLog) # RAPTRS log data as hdf5 110 | sysConfig = Loader.JsonRead(fileConfig) 111 | oData = Loader.OpenData_RAPTRS(h5Data, sysConfig) 112 | 113 | oData['cmdRoll_FF'] = h5Data['Control']['cmdRoll_pidFF'] 114 | oData['cmdRoll_FB'] = h5Data['Control']['cmdRoll_pidFB'] 115 | oData['cmdPitch_FF'] = h5Data['Control']['cmdPitch_pidFF'] 116 | oData['cmdPitch_FB'] = h5Data['Control']['cmdPitch_pidFB'] 117 | oData['cmdYaw_FF'] = h5Data['Control']['refPsi_rad'] 118 | oData['cmdYaw_FB'] = h5Data['Control']['cmdYaw_damp_rps'] 119 | 120 | # Segments 121 | rtsmSeg['seg'][1][0] += 1e6 122 | rtsmSeg['seg'][1][1] += -1e6 + 50e3 123 | 124 | oDataSegs.append(OpenData.Segment(oData, rtsmSeg['seg'])) 125 | 126 | 127 | #%% 128 | 129 | sigExcList = ['cmdRoll_rps', 'cmdPitch_rps', 'cmdYaw_rps'] 130 | sigFbList = ['cmdRoll_FB', 'cmdPitch_FB', 'cmdYaw_FB'] 131 | sigFfList = ['cmdRoll_FF', 'cmdPitch_FF', 'cmdYaw_FF'] 132 | #sigSensList = ['wB_I_rps', 'cmdPitch_FF', 'cmdYaw_FF'] 133 | 134 | freqExc_rps = [] 135 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_1']['Frequency'])) 136 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_2']['Frequency'])) 137 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_3']['Frequency'])) 138 | 139 | vCmdList = [] 140 | vExcList = [] 141 | vFbList = [] 142 | vFfList = [] 143 | ySensList = [] 144 | for iSeg, seg in enumerate(oDataSegs): 145 | vCmd = np.zeros((len(sigExcList), len(seg['time_s']))) 146 | vExc = np.zeros((len(sigExcList), len(seg['time_s']))) 147 | vFb = np.zeros((len(sigExcList), len(seg['time_s']))) 148 | vFf = np.zeros((len(sigExcList), len(seg['time_s']))) 149 | ySens = np.zeros((len(sigExcList), len(seg['time_s']))) 150 | 151 | for iSig, sigExc in enumerate(sigExcList): 152 | sigFb = sigFbList[iSig] 153 | sigFf = sigFfList[iSig] 154 | 155 | vCmd[iSig] = seg['Control'][sigExc] 156 | vExc[iSig] = seg['Excitation'][sigExc] 157 | # vFb[iSig] = seg[sigFb] 158 | vFb[iSig][1:-1] = seg[sigFb][0:-2] # Shift the time of the output into next frame 159 | vFf[iSig] = seg[sigFf] 160 | 161 | ySens[iSig] = seg['wB_I_rps'][iSig] 162 | 163 | 164 | vCmdList.append(vCmd) 165 | vExcList.append(vExc) 166 | vFbList.append(vFb) 167 | vFfList.append(vFf) 168 | ySensList.append(ySens) 169 | 170 | plt.plot(oDataSegs[iSeg]['time_s'], oDataSegs[iSeg]['vIas_mps']) 171 | 172 | plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][0]) 173 | plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][1]) 174 | plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][2]) 175 | plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][0]) 176 | plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][1]) 177 | plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][2]) 178 | 179 | 180 | #%% Estimate the frequency response function 181 | # Define the excitation frequencies 182 | freqRate_hz = 50 183 | freqRate_rps = freqRate_hz * hz2rps 184 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.2), detrendType = 'Linear') 185 | 186 | # Excited Frequencies per input channel 187 | optSpec.freq = np.asarray(freqExc_rps) 188 | 189 | # FRF Estimate 190 | LiEstNomList = [] 191 | LiEstCohList = [] 192 | svLiEstNomList = [] 193 | for iSeg, seg in enumerate(oDataSegs): 194 | 195 | freq_rps, Teb, Ceb, Pee, Pbb, Peb = FreqTrans.FreqRespFuncEst(vExcList[iSeg], vExcList[iSeg] + vFbList[iSeg], optSpec) 196 | # _ , Tev, Cev, _ , Pvv, Pev = FreqTrans.FreqRespFuncEst(vExcList[iSeg], vCmdList[iSeg], optSpec) 197 | 198 | freq_hz = freq_rps * rps2hz 199 | 200 | I3 = np.repeat([np.eye(3)], Teb.shape[-1], axis=0).T 201 | SaEstNom = Teb # Sa = I + Teb 202 | SaEstCoh = Ceb # Cxy = np.abs(Sxy)**2 / (Sxx * Syy) = (np.abs(Sxy) / Sxx) * (np.abs(Sxy) / Syy) 203 | 204 | # T = TNom = (uCtrl + uExc) / uExc - uNull / uExc 205 | # Li = inv(TNom + TUnc) - I = LiEstNom + LiEstUnc 206 | # LiEstNom = -I + TNom^-1 207 | # LiEstUnc = -(I + TNom^-1 * TUnc)^-1 * TNom^-1 * TUnc * TNom^-1 208 | LiEstNom = np.zeros_like(SaEstNom, dtype = complex) 209 | LiEstCoh = np.zeros_like(SaEstCoh) 210 | 211 | inv = np.linalg.inv 212 | 213 | for i in range(SaEstNom.shape[-1]): 214 | SaEstNomElem = SaEstNom[...,i] 215 | SaEstNomInvElem = inv(SaEstNomElem) 216 | 217 | LiEstNom[...,i] = -np.eye(3) + SaEstNomInvElem 218 | # LiEstCoh[...,i] = -np.eye(3) + inv(SaEstCoh[...,i]) 219 | LiEstCoh[...,i] = SaEstCoh[...,i] 220 | 221 | LiEstNomList.append( LiEstNom ) 222 | LiEstCohList.append( LiEstCoh ) 223 | 224 | svLiEstNomList_seg = FreqTrans.Sigma( LiEstNom ) # Singular Value Decomp 225 | svLiEstNomList.append(svLiEstNomList_seg) 226 | 227 | 228 | T_InputNames = sigExcList 229 | T_OutputNames = sigFbList 230 | 231 | # Compute Gain, Phase, Crit Distance 232 | gainLiEstNomList_mag = [] 233 | phaseLiEstNomList_deg = [] 234 | rCritLiEstNomList_mag = [] 235 | for iSeg in range(0, len(oDataSegs)): 236 | 237 | gain_mag, phase_deg = FreqTrans.GainPhase(LiEstNomList[iSeg], magUnit = 'mag', phaseUnit = 'deg', unwrap = True) 238 | 239 | gainLiEstNomList_mag.append(gain_mag) 240 | phaseLiEstNomList_deg.append(phase_deg) 241 | 242 | # rCritLiEstNom_mag, _, _ = FreqTrans.DistCrit(LiEstNomList[iSeg], typeUnc = 'ellipse') 243 | rCritLiEstNom_mag, _, _ = FreqTrans.DistCritCirc(LiEstNomList[iSeg]) 244 | 245 | rCritLiEstNomList_mag.append(rCritLiEstNom_mag) 246 | 247 | 248 | #%% Sigma Plot 249 | fig = None 250 | for iSeg in range(0, len(oDataSegs)): 251 | Cmin = np.min(np.min(LiEstCohList[iSeg], axis = 0), axis = 0) 252 | sNomMin = np.min(svLiEstNomList[iSeg], axis=0) 253 | 254 | fig = FreqTrans.PlotSigma(freq_hz[0], svLiEstNomList[iSeg], coher_nd = Cmin, fig = fig, color = rtsmSegList[iSeg]['color'], linestyle = '-', label = oDataSegs[iSeg]['Desc']) 255 | 256 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), color = 'r', linestyle = '--', fig = fig) 257 | 258 | ax = fig.get_axes() 259 | ax[0].set_xlim(0, 10) 260 | # ax[0].set_ylim(0, 1) 261 | 262 | 263 | #%% Disk Margin Plots 264 | inPlot = sigExcList # Elements of sigExcList 265 | outPlot = sigFbList # Elements of sigFbList 266 | 267 | if False: 268 | for iOut, outName in enumerate(outPlot): 269 | for iIn, inName in enumerate(inPlot): 270 | 271 | fig = None 272 | for iSeg in range(0, len(oDataSegs)): 273 | fig = FreqTrans.PlotSigma(freq_hz[0], rCritLiEstNomList_mag[iSeg][iOut, iIn], coher_nd = LiEstCohList[iSeg][iOut, iIn], fig = fig, color = rtsmSegList[iSeg]['color'], linestyle = '-', label = oDataSegs[iSeg]['Desc']) 274 | 275 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fig = fig, color = 'r', linestyle = '--') 276 | fig.suptitle(inName + ' to ' + outName, size=20) 277 | 278 | ax = fig.get_axes() 279 | # ax[0].set_ylim(0, 2) 280 | 281 | 282 | #%% Nyquist Plots 283 | if False: 284 | for iOut, outName in enumerate(outPlot): 285 | for iIn, inName in enumerate(inPlot): 286 | 287 | fig = None 288 | for iSeg in range(0, len(oDataSegs)): 289 | fig = FreqTrans.PlotNyquist(LiEstNomList[iSeg][iOut, iIn], fig = fig, color = rtsmSegList[iSeg]['color'], label = oDataSegs[iSeg]['Desc']) 290 | 291 | fig = FreqTrans.PlotNyquist(np.asarray([-1+ 0j]), TUnc = np.asarray([0.4 + 0.4j]), fig = fig, fmt = '*r', label = 'Critical Region') 292 | fig.suptitle(inName + ' to ' + outName, size=20) 293 | 294 | ax = fig.get_axes() 295 | ax[0].set_xlim(-3, 1) 296 | ax[0].set_ylim(-2, 2) 297 | 298 | 299 | #%% Bode Plots 300 | if False: 301 | for iOut, outName in enumerate(outPlot): 302 | for iIn, inName in enumerate(inPlot): 303 | 304 | fig = None 305 | for iSeg in range(0, len(oDataSegs)): 306 | fig = FreqTrans.PlotBode(freq_hz[0], gainLiEstNomList_mag[iSeg][iOut, iIn], phaseLiEstNomList_deg[iSeg][iOut, iIn], LiEstCohList[iSeg][iOut, iIn], fig = fig, color = rtsmSegList[iSeg]['color'], linestyle = '-', label = oDataSegs[iSeg]['Desc']) 307 | 308 | fig.suptitle(inName + ' to ' + outName, size=20) 309 | -------------------------------------------------------------------------------- /Examples/HuginnAeroPID.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) FLT 03 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # Constants 32 | hz2rps = 2 * np.pi 33 | rps2hz = 1 / hz2rps 34 | 35 | 36 | #%% File Lists 37 | import os.path as path 38 | 39 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 40 | #pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Huginn') 41 | 42 | fileList = {} 43 | flt = 'FLT03' 44 | fileList[flt] = {} 45 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 46 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 47 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 48 | 49 | flt = 'FLT04' 50 | fileList[flt] = {} 51 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 52 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 53 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 54 | 55 | flt = 'FLT05' 56 | fileList[flt] = {} 57 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 58 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 59 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 60 | 61 | flt = 'FLT06' 62 | fileList[flt] = {} 63 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 64 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 65 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 66 | 67 | 68 | #%% 69 | from Core import FreqTrans 70 | 71 | rtsmSegList = [ 72 | # {'flt': 'FLT03', 'seg': ('time_us', [880217573, 893797477], 'FLT03 - Sym1 - 20 m/s')}, # 20 m/s 73 | {'flt': 'FLT03', 'seg': ('time_us', [710658638, 722078703], 'FLT03 - Sym1 - 23 m/s')}, # 23 m/s 74 | # {'flt': 'FLT06', 'seg': ('time_us', [1136880543, 1146680543], 'FLT06 - Sym1 - 23 m/s')}, # 23 m/s 75 | # {'flt': 'FLT04', 'seg': ('time_us', [914627038, 926728286], 'FLT04 - Sym1 - 26 m/s')}, # 26 m/s 76 | # {'flt': 'FLT05', 'seg': ('time_us', [622279236, 634279236], 'FLT05 - Sym1 - 26 m/s')}, # 26 m/s 77 | # {'flt': 'FLT05', 'seg': ('time_us', [831211361, 843211361], 'FLT05 - Sym1 - 29 m/s')}, # 29 m/s 78 | # {'flt': 'FLT06', 'seg': ('time_us', [972425515, 984425515], 'FLT06 - Sym1 - 32 m/s')}, # 32 m/s 79 | 80 | # {'flt': 'FLT04', 'seg': ('time_us', [1054856968, 1067537708], 'FLT04 - Sym2 - 20 m/s')}, # 20 m/s 81 | # {'flt': 'FLT03', 'seg': ('time_us', [775518514, 788718440], 'FLT03 - Sym2 - 23 m/s')}, # 23 m/s 82 | # {'flt': 'FLT04', 'seg': ('time_us', [957651007, 969011852], 'FLT04 - Sym2 - 26 m/s')}, # 26 m/s 83 | 84 | # {'flt': 'FLT05', 'seg': ('time_us', [687832534, 699832534], 'FLT05 - Sym2 - 26 m/s')}, # 26 m/s 85 | # {'flt': 'FLT05', 'seg': ('time_us', [887575754, 899575754], 'FLT05 - Sym2 - 29 m/s')}, # 29 m/s 86 | # {'flt': 'FLT05', 'seg': ('time_us', [1026492809, 1036733749], 'FLT05 - Sym2 - 32 m/s')}, # 32 m/s 87 | 88 | # {'flt': 'FLT06', 'seg': ('time_us', [1122539650, 1134539650], 'FLT06 - 23 m/s')}, # 23 m/s 89 | # {'flt': 'FLT05', 'seg': ('time_us', [582408497, 594408497], 'FLT05 - 26 m/s')}, # 26 m/s 90 | # {'flt': 'FLT05', 'seg': ('time_us', [799488311, 811488311], 'FLT05 - 29 m/s')}, # 29 m/s 91 | # {'flt': 'FLT06', 'seg': ('time_us', [955822061, 967822061], 'FLT06 - 32 m/s')}, # 32 m/s 92 | ] 93 | 94 | 95 | oDataSegs = [] 96 | for rtsmSeg in rtsmSegList: 97 | fltNum = rtsmSeg['flt'] 98 | 99 | fileLog = fileList[fltNum]['log'] 100 | fileConfig = fileList[fltNum]['config'] 101 | 102 | # Load 103 | h5Data = Loader.Load_h5(fileLog) # RAPTRS log data as hdf5 104 | sysConfig = Loader.JsonRead(fileConfig) 105 | oData = Loader.OpenData_RAPTRS(h5Data, sysConfig) 106 | 107 | # Create Signal for Bending Measurement 108 | aZ = np.array([ 109 | oData['aCenterFwdIMU_IMU_mps2'][2] - oData['aCenterFwdIMU_IMU_mps2'][2][0], 110 | oData['aCenterAftIMU_IMU_mps2'][2] - oData['aCenterAftIMU_IMU_mps2'][2][0], 111 | oData['aLeftMidIMU_IMU_mps2'][2] - oData['aLeftMidIMU_IMU_mps2'][2][0], 112 | oData['aLeftFwdIMU_IMU_mps2'][2] - oData['aLeftFwdIMU_IMU_mps2'][2][0], 113 | oData['aLeftAftIMU_IMU_mps2'][2] - oData['aLeftAftIMU_IMU_mps2'][2][0], 114 | oData['aRightMidIMU_IMU_mps2'][2] - oData['aRightMidIMU_IMU_mps2'][2][0], 115 | oData['aRightFwdIMU_IMU_mps2'][2] - oData['aRightFwdIMU_IMU_mps2'][2][0], 116 | oData['aRightAftIMU_IMU_mps2'][2] - oData['aRightAftIMU_IMU_mps2'][2][0] 117 | ]) 118 | 119 | aCoefEta1 = np.array([456.1, -565.1, -289.9, 605.4, 472.4, -292, 605.6, 472.2]) * 0.3048 120 | measEta1 = aCoefEta1 @ (aZ - np.mean(aZ, axis = 0)) * 1/50 * 1e-3 121 | 122 | 123 | aCoefEta1dt = np.array([2.956, -2.998, -1.141, 5.974, 5.349, -1.149, 5.974, 5.348]) * 0.3048 124 | measEta1dt = aCoefEta1dt @ (aZ - np.mean(aZ, axis = 0)) * 1e-3 125 | 126 | # Added signals 127 | oData['measBendDt'] = measEta1dt 128 | oData['measBend'] = measEta1 129 | oData['measPitch'] = oData['wB_I_rps'][1] 130 | oData['measAlt'] = oData['altBaro_m'] 131 | 132 | 133 | oData['Control']['cmdTE1Sym_rad'] = oData['Control']['cmdTE1L_rad'] + oData['Control']['cmdTE1R_rad'] 134 | oData['Control']['cmdTE2Sym_rad'] = oData['Control']['cmdTE2L_rad'] + oData['Control']['cmdTE2R_rad'] 135 | oData['Control']['cmdTE3Sym_rad'] = oData['Control']['cmdTE3L_rad'] + oData['Control']['cmdTE3R_rad'] 136 | oData['Control']['cmdTE4Sym_rad'] = oData['Control']['cmdTE4L_rad'] + oData['Control']['cmdTE4R_rad'] 137 | oData['Control']['cmdTE5Sym_rad'] = oData['Control']['cmdTE5L_rad'] + oData['Control']['cmdTE5R_rad'] 138 | oData['Control']['cmdLESym_rad'] = oData['Control']['cmdLEL_rad'] + oData['Control']['cmdLER_rad'] 139 | 140 | oData['Excitation']['cmdTE1Sym_rad'] = oData['Excitation']['cmdTE1L_rad'] + oData['Excitation']['cmdTE1R_rad'] 141 | oData['Excitation']['cmdTE2Sym_rad'] = oData['Excitation']['cmdTE2L_rad'] + oData['Excitation']['cmdTE2R_rad'] 142 | oData['Excitation']['cmdTE3Sym_rad'] = oData['Excitation']['cmdTE3L_rad'] + oData['Excitation']['cmdTE3R_rad'] 143 | oData['Excitation']['cmdTE4Sym_rad'] = oData['Excitation']['cmdTE4L_rad'] + oData['Excitation']['cmdTE4R_rad'] 144 | oData['Excitation']['cmdTE5Sym_rad'] = oData['Excitation']['cmdTE5L_rad'] + oData['Excitation']['cmdTE5R_rad'] 145 | oData['Excitation']['cmdLESym_rad'] = oData['Excitation']['cmdLEL_rad'] + oData['Excitation']['cmdLER_rad'] 146 | 147 | oData['Control']['cmdTE1Sym_rad'] = oData['Control']['cmdTE1L_rad'] + oData['Control']['cmdTE1R_rad'] 148 | oData['Control']['cmdTE2Sym_rad'] = oData['Control']['cmdTE2L_rad'] + oData['Control']['cmdTE2R_rad'] 149 | oData['Control']['cmdTE3Sym_rad'] = oData['Control']['cmdTE3L_rad'] + oData['Control']['cmdTE3R_rad'] 150 | oData['Control']['cmdTE4Sym_rad'] = oData['Control']['cmdTE4L_rad'] + oData['Control']['cmdTE4R_rad'] 151 | oData['Control']['cmdTE5Sym_rad'] = oData['Control']['cmdTE5L_rad'] + oData['Control']['cmdTE5R_rad'] 152 | oData['Control']['cmdLESym_rad'] = oData['Control']['cmdLEL_rad'] + oData['Control']['cmdLER_rad'] 153 | 154 | oData['Surf'] = {} 155 | for surfName in h5Data['Sensors']['Surf'].keys(): 156 | oData['Surf'][surfName] = h5Data['Sensors']['Surf'][surfName]['CalibratedValue'] 157 | 158 | oData['Surf']['posTE1Sym'] = oData['Surf']['posTE1L'] + oData['Surf']['posTE1R'] 159 | oData['Surf']['posTE2Sym'] = oData['Surf']['posTE2L'] + oData['Surf']['posTE2R'] 160 | oData['Surf']['posTE3Sym'] = oData['Surf']['posTE3L'] + oData['Surf']['posTE3R'] 161 | oData['Surf']['posTE4Sym'] = oData['Surf']['posTE4L'] + oData['Surf']['posTE4R'] 162 | oData['Surf']['posTE5Sym'] = oData['Surf']['posTE5L'] + oData['Surf']['posTE5R'] 163 | oData['Surf']['posLESym'] = oData['Surf']['posLEL'] + oData['Surf']['posLER'] 164 | 165 | # Segments 166 | seg = OpenData.Segment(oData, rtsmSeg['seg']) 167 | oDataSegs.append(seg) 168 | 169 | # plt.plot(seg['time_s'], seg['Excitation']['cmdTE1L_rad'], seg['time_s'], seg['Excitation']['cmdTE2L_rad'], seg['time_s'], seg['measBend'], seg['time_s'], seg['measPitch']) 170 | plt.plot(seg['time_s'], seg['measPitch'], seg['time_s'], seg['measBend']) 171 | 172 | 173 | #%% 174 | 175 | sigInList = ['cmdTE1Sym_rad', 'cmdTE3Sym_rad', 'cmdTE5Sym_rad'] 176 | #sigInList = ['cmdTE2Sym_rad', 'cmdTE4Sym_rad', 'cmdLESym_rad'] 177 | #sigInList = ['posTE1Sym', 'posTE3Sym', 'posTE5Sym'] 178 | #sigInList = ['cmdRoll_rps', 'cmdPitch_rps', 'cmdBend_nd'] 179 | 180 | sigOutList = ['measPitch', 'measBend'] 181 | 182 | 183 | freqExc_rps = [] 184 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_Surf_1']['Frequency'])) 185 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_Surf_2']['Frequency'])) 186 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_Surf_3']['Frequency'])) 187 | 188 | plt.figure() 189 | 190 | eList = [] 191 | uList = [] 192 | #inSigList = [] 193 | for iSeg, seg in enumerate(oDataSegs): 194 | eSig = np.zeros((len(sigInList), len(seg['time_s']))) 195 | uSig = np.zeros((len(sigInList), len(seg['time_s']))) 196 | 197 | for iSig, sigIn in enumerate(sigInList): 198 | eSig[iSig] = seg['Excitation'][sigIn] 199 | uSig[iSig] = seg['Control'][sigIn] 200 | # inSig[iSig] = seg['Surf'][sigIn] 201 | 202 | plt.plot(oDataSegs[iSeg]['time_s'], eSig[iSig], oDataSegs[iSeg]['time_s'], uSig[iSig]) 203 | 204 | eList.append(eSig) 205 | uList.append(uSig) 206 | # inSigList.append(inSig) 207 | 208 | 209 | outList = [] 210 | for iSeg, seg in enumerate(oDataSegs): 211 | outSig = np.zeros((len(sigOutList), len(seg['time_s']))) 212 | 213 | for iSig, sigOut in enumerate(sigOutList): 214 | outSig[iSig] = seg[sigOut] 215 | # outSig[iSig] = seg['Surf'][sigOut] 216 | 217 | # plt.plot(oDataSegs[iSeg]['time_s'], outSig[iSig]) 218 | 219 | outList.append(outSig) 220 | 221 | 222 | #%% Estimate the frequency response function 223 | # Define the excitation frequencies 224 | freqRate_hz = 50 225 | freqRate_rps = freqRate_hz * hz2rps 226 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.2), detrendType = 'Linear') 227 | 228 | # Excited Frequencies per input channel 229 | optSpec.freq = np.asarray(freqExc_rps) 230 | 231 | # FRF Estimate 232 | freq_rps = [] 233 | freq_hz = [] 234 | T = [] 235 | C = [] 236 | for iSeg, seg in enumerate(oDataSegs): 237 | 238 | freq, Tey, Cey, Pee, Pyy, Pey = FreqTrans.FreqRespFuncEst(eList[iSeg], outList[iSeg], optSpec) 239 | freq, Teu, Ceu, _, Puu, Peu = FreqTrans.FreqRespFuncEst(eList[iSeg], uList[iSeg], optSpec) 240 | 241 | # Form the Frequency Response 242 | freq_hz = freq * rps2hz 243 | 244 | # Form the Frequency Response 245 | T_seg = np.zeros_like(Tey) 246 | 247 | for i in range(T_seg.shape[-1]): 248 | T_seg[...,i] = Tey[...,i] @ np.linalg.inv(Teu[...,i]) 249 | 250 | 251 | T.append( T_seg ) 252 | C.append(Cey) 253 | 254 | 255 | T_InputNames = sigInList 256 | T_OutputNames = sigOutList 257 | 258 | # Compute Gain, Phase, Crit Distance 259 | gain_mag = [] 260 | phase_deg = [] 261 | for iSeg in range(0, len(oDataSegs)): 262 | gain_mag.append(FreqTrans.Gain(T[iSeg], magUnit = 'mag')) 263 | phase_deg.append(FreqTrans.Phase(T[iSeg], phaseUnit = 'deg', unwrap = False)) 264 | 265 | 266 | #%% Spectrograms 267 | if False: 268 | 269 | iSgnl = 0 270 | 271 | freqRate_rps = 50 * hz2rps 272 | 273 | # freqSpec = np.asarray(freqExc_rps).flatten() 274 | freqSpec = freqExc_rps[iSgnl] 275 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freq = freqSpec, freqRate = freqRate_rps, winType = ('tukey', 0.2), smooth = ('box', 3), detrendType = 'Linear') 276 | 277 | 278 | for iSeg in range(0, len(oDataSegs)): 279 | t = oDataSegs[iSeg]['time_s'] 280 | y = outList[iSeg][iSgnl] 281 | 282 | # Number of time segments and length of overlap, units of samples 283 | #lenSeg = 2**6 - 1 284 | lenSeg = int(1 * optSpec.freqRate * rps2hz) 285 | lenOverlap = 5 286 | 287 | # Compute Spectrum over time 288 | tSpec_s, freqSpec_rps, P_mag = FreqTrans.SpectTime(t, y, lenSeg, lenOverlap, optSpec) 289 | 290 | # Plot the Spectrogram 291 | fig = FreqTrans.Spectogram(tSpec_s, freqSpec_rps * rps2hz, 20 * np.log10(P_mag)) 292 | fig.suptitle(oDataSegs[iSeg]['Desc'] + ': Spectrogram - ' + sigOutList[iSgnl]) 293 | 294 | 295 | #%% Nyquist Plots 296 | inPlot = sigInList # Elements of sigInList 297 | outPlot = sigOutList # Elements of sigOutList 298 | 299 | if False: 300 | for iOut, outName in enumerate(outPlot): 301 | for iIn, inName in enumerate(inPlot): 302 | 303 | fig = None 304 | for iSeg in range(0, len(oDataSegs)): 305 | fig = FreqTrans.PlotNyquist(T[iSeg][iOut, iIn], fig = fig, fmt = '*', label = oDataSegs[iSeg]['Desc']) 306 | 307 | fig.suptitle(inName + ' to ' + outName, size=20) 308 | 309 | ax = fig.get_axes() 310 | ax[0].set_xlim(-3, 1) 311 | ax[0].set_ylim(-2, 2) 312 | 313 | 314 | #%% Bode Plots 315 | if True: 316 | 317 | for iOut, outName in enumerate(outPlot): 318 | for iIn, inName in enumerate(inPlot): 319 | 320 | fig = None 321 | for iSeg in range(0, len(oDataSegs)): 322 | fig = FreqTrans.PlotBode(freq_hz[0], gain_mag[iSeg][iOut, iIn], phase_deg[iSeg][iOut, iIn], C[iSeg][iOut, iIn], dB = False, fig = fig, fmt = '*--', label = oDataSegs[iSeg]['Desc']) 323 | 324 | fig.suptitle(inName + ' to ' + outName, size = 20) 325 | -------------------------------------------------------------------------------- /Core/GenExcite.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | """ 9 | 10 | import numpy as np 11 | 12 | # Constants 13 | hz2rps = 2*np.pi 14 | rps2hz = 1/hz2rps 15 | 16 | 17 | #%% Step, Pulse, Doublet, 3-2-1-1 18 | def Discrete(): 19 | 20 | return 21 | 22 | #%% Linear and Log Sweeps/Chirps 23 | def Chirp(freqInit_rps, freqFinal_rps, time_s, ampInit = 1.0, ampFinal = 1.0, freqType = 'linear', ampType = 'linear', initZero = 1): 24 | # Check Inputs 25 | timeRate_s = time_s[2] - time_s[1] 26 | freqMaxLimit_rps = (1/(2*timeRate_s) * hz2rps) 27 | 28 | 29 | if freqInit_rps > freqMaxLimit_rps: 30 | print('Chirp - The initial frequency is too high for the frame rate') 31 | freqInit_rps = freqMaxLimit_rps 32 | 33 | if freqFinal_rps > freqMaxLimit_rps: 34 | print('Chirp - The final desired frequency is too high for the frame rate') 35 | freqFinal_rps = freqMaxLimit_rps 36 | 37 | # Number of time samples 38 | numSamples = len(time_s) 39 | 40 | # End time 41 | timeFinal_s = time_s[-1:] 42 | 43 | # Phase variation and Frequency variation 44 | if freqType in 'linear': 45 | freq_rps = ((freqFinal_rps-freqInit_rps)/(2*timeFinal_s) * time_s) + freqInit_rps 46 | phase_rad = freq_rps * time_s 47 | 48 | elif freqType in 'log10': 49 | phase_rad = ((timeFinal_s*freqInit_rps) / np.log10(freqFinal_rps/freqInit_rps)) * ((freqFinal_rps/freqInit_rps)**(time_s/timeFinal_s) - 1) 50 | freq_rps = phase_rad / time_s 51 | 52 | elif freqType in 'ln': 53 | phase_rad = ((timeFinal_s*freqInit_rps) / np.log(freqFinal_rps/freqInit_rps)) * ((freqFinal_rps/freqInit_rps)**(time_s/timeFinal_s) - 1) 54 | freq_rps = phase_rad / time_s 55 | 56 | else: 57 | print('Chirp - Unkown frequency variation type') 58 | 59 | 60 | # Amplitude variation 61 | iSample = np.linspace(0, numSamples-1, numSamples) 62 | if ampType in 'linear': 63 | amp = ampInit + iSample * ((ampFinal - ampInit)/(numSamples - 1)) 64 | 65 | elif ampType in 'log10': 66 | amp = 10**(np.log10(ampInit) + iSample * (np.log10(ampFinal) - np.log10(ampInit)) / (numSamples-1)) 67 | 68 | elif ampType in 'ln': 69 | amp = np.exp(1)**(np.log(ampInit) + iSample * (np.log(ampFinal) - np.log(ampInit)) / (numSamples-1)) 70 | 71 | else: 72 | print('Chirp - Unkown amplitude variation type') 73 | 74 | 75 | # Generate chirp time history 76 | signal = amp * np.sin(phase_rad) 77 | 78 | 79 | # Ensure a zero final value 80 | if initZero: 81 | # Find the index imediately before the last zero crossing 82 | iForceZero = np.where(np.abs(np.diff(np.sign(signal))))[0][-1] + 1 83 | 84 | # Hold zero after the last zero crossing 85 | signal[iForceZero:] = 0.0 86 | 87 | 88 | # Return 89 | return (signal, amp, freq_rps) 90 | 91 | 92 | 93 | #%% Peak Minimal Optimal Multisine 94 | def MultiSine(freqElem_rps, ampElem_nd, sigIndx, time_s, phaseInit_rad = 0, boundPhase = True, initZero = True, normalize = None, costType = 'Schoeder'): 95 | 96 | #Reference: 97 | # "Synthesis of Low-Peak-Factor Signals and Binary Sequences with Low 98 | # Autocorrelation", Schroeder, IEEE Transactions on Information Theory 99 | # Jan 1970. 100 | # 101 | # "Tailored Excitation for Multivariable Stability Margin Measurement 102 | # Applied to the X-31A Nonlinear Simulation" NASA TM-113085 103 | # John Bosworth and John Burken 104 | 105 | # 106 | # "Multiple Input Design for Real-Time Parameter Estimation" 107 | # Eugene A. Morelli, 2003 108 | 109 | 110 | if costType.lower() == 'schroeder': # Schroeder Wave 111 | phaseElem_rad = SchroederPhase(ampElem_nd, phaseInit_rad, boundPhase) 112 | 113 | elif costType.lower() in ['oms', 'norm2', 'squaresum', 'max']: # Optimal Multisine Wave for minimum peak factor 114 | if costType.lower() == 'oms': 115 | costType = 'norm2' 116 | 117 | phaseElem_rad = OptimalPhase(freqElem_rps, ampElem_nd, sigIndx, time_s, phaseInit_rad, boundPhase, costType) 118 | 119 | 120 | # Generate the signals 121 | [sigList, sigElem] = MultiSineAssemble(freqElem_rps, phaseElem_rad, ampElem_nd, time_s, sigIndx) 122 | 123 | # Offset the phase components to yield near zero initial and final values 124 | # for each of the signals, based on Morrelli. This is optional. 125 | if initZero: 126 | for i in range(10): # Do this a few times to improve the results 127 | # Phase shift required for each of the frequency components 128 | phaseElem_rad += PhaseShift(sigList, time_s, freqElem_rps, sigIndx) 129 | 130 | # Recompute the signals with the phases 131 | [sigList, sigElem] = MultiSineAssemble(freqElem_rps, phaseElem_rad, ampElem_nd, time_s, sigIndx) 132 | 133 | 134 | # Re-scale and re-assemble to achieve desired normalization 135 | if normalize == 'peak': 136 | # unity peak-to-peak amplitude on each channel 137 | for iSig, sig in enumerate(sigList): 138 | iElem = sigIndx[iSig] 139 | ampElem_nd[iElem] *= 2.0 / (max(sig) - min(sig)) 140 | 141 | [sigList, sigElem] = MultiSineAssemble(freqElem_rps, phaseElem_rad, ampElem_nd, time_s, sigIndx) 142 | 143 | elif normalize == 'rms': 144 | # unity Root-Mean-Square amplitude on each channel 145 | for iSig, sig in enumerate(sigList): 146 | iElem = sigIndx[iSig] 147 | ampElem_nd[iElem] *= 1.0 / np.sqrt(np.mean(sig**2)) 148 | 149 | [sigList, sigElem] = MultiSineAssemble(freqElem_rps, phaseElem_rad, ampElem_nd, time_s, sigIndx) 150 | 151 | return np.asarray(sigList), phaseElem_rad, sigElem 152 | 153 | 154 | 155 | #%% SchroederPhase 156 | def SchroederPhase(ampElem_nd, phaseInit_rad = 0, boundPhase = True): 157 | 158 | #Reference: 159 | # "Synthesis of Low-Peak-Factor Signals and Binary Sequences with Low 160 | # Autocorrelation", Schroeder, IEEE Transactions on Information Theory 161 | # Jan 1970. 162 | 163 | # Compute the relative signal power 164 | sigPowerRel = (ampElem_nd / max(ampElem_nd))**2 / len(ampElem_nd) 165 | 166 | # Initialize the phase elements 167 | phaseElem_rad = np.zeros_like(ampElem_nd) 168 | phaseElem_rad[0] = phaseInit_rad 169 | 170 | # Compute the Schroeder phase shifts 171 | # Compute phases (Reference 1, Equation 11) 172 | numElem = len(phaseElem_rad) 173 | for iElem in range(1, numElem): 174 | sumVal = 0; 175 | for indxL in range(1, iElem): 176 | sumVal = sumVal + ((iElem - indxL) * sigPowerRel[indxL]) 177 | 178 | phaseElem_rad[iElem] = phaseElem_rad[0] - 2*np.pi * sumVal 179 | 180 | # Bound Phases 181 | # Correct phases to be in the range [0, 2*pi), optional 182 | if boundPhase: 183 | phaseElem_rad = np.mod(phaseElem_rad, 2*np.pi); 184 | 185 | 186 | return phaseElem_rad 187 | 188 | 189 | #%% Optimal Phase 190 | def OptimalPhase(freqElem_rps, ampElem_nd, sigIndx, time_s, phaseInit_rad = 0, boundPhase = 1, costType = 'norm2'): 191 | #Reference: 192 | # "Multiple Input Design for Real-Time Parameter Estimation" 193 | # Eugene A. Morelli, 2003 194 | 195 | from scipy.optimize import minimize 196 | 197 | if sigIndx is None: 198 | sigIndx = np.ones_like(ampElem_nd) 199 | 200 | # Initial Guess for Phase based on Schroeder 201 | phaseElem_rad = SchroederPhase(ampElem_nd, phaseInit_rad, boundPhase = 1) 202 | #phaseElem_rad = np.zeros_like(ampElem_nd) 203 | #phaseElem_rad = np.random.normal(0.0, np.pi, (ampElem_nd.shape)) 204 | 205 | # Define the Cost Function for the Optimization 206 | def OMSCostFunc(phaseElem_rad, freqElem_rps, ampElem_nd, time_s, sigIndx, costType = 'norm2'): 207 | sigList, sigElem = MultiSineAssemble(freqElem_rps, phaseElem_rad, ampElem_nd, time_s, sigIndx) 208 | peakfactor = PeakFactor(sigList) 209 | 210 | # Cost Type 211 | cost = [] 212 | if costType.lower() == 'norm2': 213 | cost = np.linalg.norm(peakfactor, 2) 214 | 215 | elif costType.lower() == 'squaresum': 216 | cost = sum(peakfactor**2) 217 | 218 | elif costType.lower() == 'max': 219 | cost = max(peakfactor) 220 | 221 | return cost 222 | 223 | # Compute the optimal phases 224 | optMethod = 'BFGS' 225 | #optOptions = {'maxiter': 100, 'disp': True} 226 | optOptions = {'maxiter': 400, 'disp': True} 227 | #optOptions = {'disp': True} 228 | 229 | optResult = minimize(OMSCostFunc, phaseElem_rad, args = (freqElem_rps, ampElem_nd, time_s, sigIndx, costType), method = optMethod, options = optOptions) 230 | phaseElem_rad = np.copy(optResult.x) 231 | 232 | 233 | # Bound Phases 234 | # Correct phases to be in the range [0, 2*pi), optional 235 | if boundPhase: 236 | phaseElem_rad = np.mod(phaseElem_rad, 2*np.pi); 237 | 238 | 239 | return phaseElem_rad 240 | 241 | 242 | #%% MultiSineAssemble 243 | def MultiSineAssemble(freqElem_rps, phaseElem_rad, ampElem_nd, time_s, sigIndx = None): 244 | 245 | # Default Values and Constants 246 | if sigIndx is None: 247 | sigIndx = np.ones_like(freqElem_rps) 248 | 249 | # Check Inputs 250 | if (len(freqElem_rps) != len(phaseElem_rad)) or (len(freqElem_rps) != len(ampElem_nd)): 251 | print('MultiSineAssemble - Inputs for signal power, frequency, and phase must have the same length') 252 | 253 | # Generate each signal component 254 | numChan = len(sigIndx) 255 | numElem = len(freqElem_rps) 256 | sigElem = np.zeros((numElem, len(time_s))) 257 | for iElem in range(0, numElem): 258 | sigElem[iElem] = ampElem_nd[iElem] * np.sin(freqElem_rps[iElem] * time_s + phaseElem_rad[iElem]) 259 | 260 | 261 | # Combine signal components into signals 262 | sigList = [] 263 | for iChan in range(0, numChan): 264 | iElem = sigIndx[iChan] 265 | sig = sum(sigElem[iElem]) 266 | sigList.append(sig) 267 | 268 | return sigList, sigElem 269 | 270 | #%% 271 | def MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_rps, numCycles = 1, freqStepDes_rps = 0, methodSW = 'zipper'): 272 | 273 | ## Check Inputs 274 | if len(freqMinDes_rps) is not len(freqMaxDes_rps): 275 | print('MultiSineComponents - The min and max frequency inputs must be the same length') 276 | 277 | # Number of channels 278 | numChan = len(freqMinDes_rps) 279 | 280 | freqMaxLimit_rps = freqRate_rps / 2 281 | if any(freqMaxDes_rps > freqMaxLimit_rps): 282 | print('MultiSineComponents - The maximum desired frequency is too high for the frame rate'); 283 | freqMaxDes_rps = freqMaxLimit_rps; 284 | 285 | 286 | ## Refine the frequency selection to avoid leakage, based on Bosworth 287 | # Convert frequencies from rad/s to Hz 288 | freqMinDes_hz = freqMinDes_rps * rps2hz 289 | freqMaxDes_hz = freqMaxDes_rps * rps2hz 290 | 291 | # Time vector is based on completing the desired number of cycles for the 292 | # min frequency component, must be divisible by the frame rate 293 | freqRate_hz = freqRate_rps * rps2hz 294 | timeDur_s = (round((numCycles / min(freqMinDes_hz))*freqRate_hz)) / freqRate_hz 295 | time_s = np.linspace(0, timeDur_s, int(timeDur_s*freqRate_hz) + 1) 296 | 297 | 298 | # Frequency sequence step size 299 | freqStepMin_hz = 1/freqRate_hz # Absolute Minimum (tightest spacing) step size 300 | freqStepMax_hz = min(freqMaxDes_hz - freqMinDes_hz) # Max - Min would be a huge step size 301 | 302 | freqStepDes_hz = freqStepDes_rps * rps2hz 303 | freqStep_hz = np.round(freqStepDes_hz / freqStepMin_hz) * freqStepMin_hz 304 | # freqStep_hz = freqStepDes_hz 305 | 306 | # Clip if required 307 | freqStep_hz = np.clip(freqStep_hz, freqStepMin_hz, freqStepMax_hz) 308 | 309 | # Adjust the min frequency based on the max time 310 | freqMin_hz = numCycles / timeDur_s 311 | freqMax_hz = np.max(freqMaxDes_hz) 312 | 313 | # Frequencies of all the components 314 | freqElem_hz = np.arange(freqMin_hz, freqMax_hz, freqStep_hz) 315 | freqMax_hz = np.max(freqElem_hz) 316 | numElem = len(freqElem_hz) 317 | freqElem_rps = freqElem_hz * hz2rps 318 | 319 | ## Distribute the frequency components into the signals 320 | # Distribution methods 321 | if methodSW in ['zipper', 'zip']: 322 | # Zippered distribution 323 | numElem = int(np.floor(numElem/numChan) * numChan) # truncate 324 | sigIndx = np.zeros((numChan, int(numElem/numChan)), dtype=int) 325 | 326 | for iSignal in range(0, numChan): 327 | sigIndx[iSignal, ] = range(iSignal, numElem, numChan) 328 | 329 | else: 330 | print('MultiSineComponent - Distribution method type not understood') 331 | 332 | return freqElem_rps, sigIndx, time_s 333 | 334 | 335 | #%% PhaseShift 336 | def PhaseShift(sigList, time_s, freqElem_rps, sigIndx): 337 | 338 | #Reference: 339 | # "Multiple Input Design for Real-Time Parameter Estimation" 340 | # Eugene A. Morelli, 2003 341 | # 342 | 343 | ## Check Inputs 344 | # Number of signals and components 345 | numChan = len(sigIndx) 346 | 347 | ## Time shift per signal 348 | # Find the timeShift require so that each channel start near zero, then compute the phase for each element 349 | phaseShift_rad = np.zeros_like(freqElem_rps) 350 | for iChan in range(0, numChan): 351 | iSig = sigIndx[iChan] 352 | sig = sigList[iChan] 353 | 354 | # Find the first zero crossing 355 | iZero = np.where(np.abs(np.diff(np.sign(sig))))[0][0] 356 | 357 | # Refine the solution via interpolation around the sign switch, and return the time shift 358 | timeShift_s = np.interp(0, sig[iZero:iZero+2], time_s[iZero:iZero+2]) 359 | 360 | # Find the phase shift associated with the time shift for each frequency component in the combined signals 361 | phaseShift_rad[iSig] = timeShift_s * freqElem_rps[iSig] 362 | 363 | 364 | return phaseShift_rad 365 | 366 | #%% PeakFactor before and/or after 367 | def PeakFactor(sigList): 368 | 369 | ## Calculate the peak factor 370 | numChan = len(sigList) 371 | peakFactor = np.zeros(numChan) 372 | for iChan in range(0, numChan): 373 | sig = sigList[iChan] 374 | peakFactor[iChan] = (max(sig) - min(sig)) / (2*np.sqrt(np.dot(sig, sig) / len(sig))) 375 | 376 | 377 | return peakFactor 378 | -------------------------------------------------------------------------------- /Core/Loader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Louis Mueller 5 | University of Minnesota UAV Lab 6 | 7 | Description: 8 | Read and rename flight data .h5 file. 9 | 10 | """ 11 | import h5py 12 | import json 13 | import csv 14 | 15 | import numpy as np 16 | 17 | 18 | #%% Load HDF5 into Dictionary 19 | def Load_h5(filename): 20 | with h5py.File(filename, 'r') as f: 21 | data = LoadRecursive_h5(f, '/') 22 | return data 23 | 24 | def LoadRecursive_h5(f, basePath): 25 | data = {} 26 | for key, item in f[basePath].items(): 27 | # print(key) 28 | # print(item) 29 | if isinstance(item, h5py._hl.dataset.Dataset): 30 | # print('Dataset') 31 | data[key] = item[()] 32 | data[key] = data[key].flatten() 33 | elif isinstance(item, h5py._hl.group.Group): 34 | # print('Group') 35 | data[key] = LoadRecursive_h5(f, basePath + key + '/') 36 | return data 37 | 38 | 39 | #%% Read/Write Json files into Dictionary 40 | def JsonRead(filename): 41 | with open(filename, 'r') as f: 42 | data = json.load(f) 43 | f.close() 44 | return data 45 | 46 | def JsonWrite(filename, data): 47 | with open(filename, 'w') as f: 48 | json.dump(data, f, indent = 4, ensure_ascii=False) 49 | f.close() 50 | return 1 51 | 52 | 53 | #%% HDF5 Read 54 | def Log_RAPTRS(filename, fileConfig): 55 | 56 | h5Data = Load_h5(filename) # RAPTRS log data as hdf5 57 | 58 | if 'PostProcess' in h5Data['Sensor-Processing']: 59 | for key in h5Data['Sensor-Processing']['PostProcess']['INS'].keys(): 60 | h5Data['Sensor-Processing'][key] = h5Data['Sensor-Processing']['PostProcess']['INS'][key] 61 | 62 | sysConfig = JsonRead(fileConfig) 63 | oData = OpenData_RAPTRS(h5Data, sysConfig) 64 | 65 | return oData, h5Data 66 | 67 | #%% Convert to OpenData Format 68 | def OpenData_RAPTRS(h5Data, sysConfig, oData = {}): 69 | 70 | r2d = 180/np.pi; 71 | 72 | # TIME 73 | oData['time_us'] = h5Data['Sensors']['Fmu']['Time_us'] 74 | oData['time_s'] = oData['time_us'] * 1e-6 75 | oData['timeDiff_s'] = np.diff(oData['time_s'], prepend=oData['time_s'][0]) 76 | 77 | # IMU (a, w, mag) 78 | oData['aImu_I_mps2'] = np.array([h5Data['Sensors']['Fmu']['Mpu9250']['AccelX_mss'],h5Data['Sensors']['Fmu']['Mpu9250']['AccelY_mss'],h5Data['Sensors']['Fmu']['Mpu9250']['AccelZ_mss']]) 79 | oData['wImu_I_rps'] = np.array([h5Data['Sensors']['Fmu']['Mpu9250']['GyroX_rads'],h5Data['Sensors']['Fmu']['Mpu9250']['GyroY_rads'],h5Data['Sensors']['Fmu']['Mpu9250']['GyroZ_rads']]) 80 | oData['magImu_L_uT'] = np.array([h5Data['Sensors']['Fmu']['Mpu9250']['MagX_uT'],h5Data['Sensors']['Fmu']['Mpu9250']['MagY_uT'],h5Data['Sensors']['Fmu']['Mpu9250']['MagZ_uT']]) 81 | 82 | if 'Imu' in h5Data['Sensors']: 83 | for imuName in h5Data['Sensors']['Imu'].keys(): 84 | if imuName in h5Data['Sensors']['Imu']: 85 | oData['a'+imuName+'IMU_IMU_mps2'] = np.array([h5Data['Sensors']['Imu'][imuName]['AccelX_mss'],h5Data['Sensors']['Imu'][imuName]['AccelY_mss'],h5Data['Sensors']['Imu'][imuName]['AccelZ_mss']]) 86 | oData['w'+imuName+'IMU_IMU_rps'] = np.array([h5Data['Sensors']['Imu'][imuName]['GyroX_rads'],h5Data['Sensors']['Imu'][imuName]['GyroY_rads'],h5Data['Sensors']['Imu'][imuName]['GyroZ_rads']]) 87 | 88 | # Thor Pitot-Static 89 | if 'Swift' in h5Data['Sensors']: 90 | oData['pTip_Pa'] = h5Data['Sensors']['Swift']['Differential']['Pressure_Pa'] 91 | oData['pStatic_Pa'] = h5Data['Sensors']['Swift']['Static']['Pressure_Pa'] 92 | oData['tempProbe_C'] = np.mean([h5Data['Sensors']['Swift']['Differential']['Temperature_C'], h5Data['Sensors']['Swift']['Static']['Temperature_C']], axis=0) 93 | 94 | if 'Pitot' in h5Data['Sensors']: 95 | oData['pTip_Pa'] = h5Data['Sensors']['Pitot']['Differential']['Pressure_Pa'] 96 | oData['pStatic_Pa'] = h5Data['Sensors']['Pitot']['Static']['Pressure_Pa'] 97 | oData['tempProbe_C'] = np.mean([h5Data['Sensors']['Pitot']['Differential']['Temperature_C'], h5Data['Sensors']['Pitot']['Static']['Temperature_C']], axis=0) 98 | 99 | # Huginn Pitot-Static 100 | if '5Hole' in h5Data['Sensors']: 101 | if 'Alpha1' in h5Data['Sensors']['5Hole']: # For Huginn 102 | oData['pAlpha1_Pa'] = h5Data['Sensors']['5Hole']['Alpha1']['Pressure_Pa'] 103 | oData['pAlpha2_Pa'] = h5Data['Sensors']['5Hole']['Alpha2']['Pressure_Pa'] 104 | oData['pBeta1_Pa'] = h5Data['Sensors']['5Hole']['Beta1']['Pressure_Pa'] 105 | oData['pBeta2_Pa'] = h5Data['Sensors']['5Hole']['Beta2']['Pressure_Pa'] 106 | oData['pStatic_Pa'] = h5Data['Sensors']['5Hole']['Static']['Pressure_Pa'] 107 | oData['pTip_Pa'] = h5Data['Sensors']['5Hole']['Tip']['Pressure_Pa'] 108 | oData['tempProbe_C'] = np.mean( 109 | [h5Data['Sensors']['5Hole']['Alpha1']['Temperature_C'], 110 | h5Data['Sensors']['5Hole']['Alpha2']['Temperature_C'], 111 | h5Data['Sensors']['5Hole']['Beta1']['Temperature_C'], 112 | h5Data['Sensors']['5Hole']['Beta2']['Temperature_C'], 113 | h5Data['Sensors']['5Hole']['Static']['Temperature_C'], 114 | h5Data['Sensors']['5Hole']['Tip']['Temperature_C']], axis=0) 115 | 116 | 117 | # Mjolnir Pitot-Static 118 | if 'PresAlpha' in h5Data['Sensors']['5Hole']: # For Mjolnir 119 | oData['pAlpha_Pa'] = h5Data['Sensors']['5Hole']['PresAlpha']['Pressure_Pa'] 120 | oData['pBeta_Pa'] = h5Data['Sensors']['5Hole']['PresBeta']['Pressure_Pa'] 121 | oData['pStatic_Pa'] = h5Data['Sensors']['5Hole']['Static']['Pressure_Pa'] 122 | oData['pTip_Pa'] = h5Data['Sensors']['5Hole']['Tip']['Pressure_Pa'] 123 | oData['tempProbe_C'] = np.mean( 124 | [h5Data['Sensors']['5Hole']['PresAlpha']['Temperature_C'], 125 | h5Data['Sensors']['5Hole']['PresBeta']['Temperature_C'], 126 | h5Data['Sensors']['5Hole']['Static']['Temperature_C'], 127 | h5Data['Sensors']['5Hole']['Tip']['Temperature_C']], axis=0) 128 | 129 | # Airdata 130 | oData['vIas_mps'] = h5Data['Sensor-Processing']['vIAS_ms'] 131 | oData['altBaro_m'] = h5Data['Sensor-Processing']['hBaro_m'] 132 | 133 | if 'alpha_rad' in h5Data['Sensor-Processing']: 134 | oData['alpha_rad'] = h5Data['Sensor-Processing']['alpha_rad'] 135 | 136 | if 'beta_rad' in h5Data['Sensor-Processing']: 137 | oData['beta_rad'] = h5Data['Sensor-Processing']['beta_rad'] 138 | 139 | # Controllers 140 | oData['refPhi_rad'] = h5Data['Control']['refPhi_rad'] 141 | oData['refTheta_rad'] = h5Data['Control']['refTheta_rad'] 142 | if 'refPsi_rad' in h5Data['Control']: 143 | oData['refPsi_rad'] = h5Data['Control']['refPsi_rad'] 144 | oData['refV_mps'] = h5Data['Control']['refV_ms'] 145 | oData['refH_m'] = h5Data['Control']['refH_m'] 146 | 147 | # Effectors 148 | # Get the list of effectors 149 | # sysConfig['Effectors'] 150 | effList = ['cmdMotor_nd', 'cmdElev_rad', 'cmdRud_rad', 'cmdAilL_rad', 'cmdAilR_rad', 'cmdFlapL_rad', 'cmdFlapR_rad', 151 | 'cmdTE1L_rad', 'cmdTE1R_rad', 'cmdTE2L_rad', 'cmdTE2R_rad', 'cmdTE3L_rad', 'cmdTE3R_rad', 'cmdTE4L_rad', 'cmdTE4R_rad', 'cmdTE5L_rad', 'cmdTE5R_rad', 'cmdLEL_rad', 'cmdLER_rad'] 152 | 153 | oData['Effectors'] = {} 154 | for eff in effList: 155 | if eff in h5Data['Control']: 156 | oData['Effectors'][eff] = h5Data['Control'][eff] 157 | 158 | # GPS 159 | oData['rGps_D_ddm'] = np.array([h5Data['Sensors']['uBlox']['Latitude_rad'] * r2d, h5Data['Sensors']['uBlox']['Longitude_rad'] * r2d, h5Data['Sensors']['uBlox']['Altitude_m']]) 160 | oData['vGps_L_mps'] = np.array([h5Data['Sensors']['uBlox']['NorthVelocity_ms'], h5Data['Sensors']['uBlox']['EastVelocity_ms'], h5Data['Sensors']['uBlox']['DownVelocity_ms']]) 161 | 162 | # EKF 163 | oData['rB_D_ddm'] = np.array([h5Data['Sensor-Processing']['Latitude_rad'] * r2d, h5Data['Sensor-Processing']['Longitude_rad'] * r2d, h5Data['Sensor-Processing']['Altitude_m']]) 164 | oData['vB_L_mps'] = np.array([h5Data['Sensor-Processing']['NorthVelocity_ms'], h5Data['Sensor-Processing']['EastVelocity_ms'], h5Data['Sensor-Processing']['DownVelocity_ms']]) 165 | 166 | oData['aB_I_mps2'] = np.array([h5Data['Sensor-Processing']['AccelX_mss'], h5Data['Sensor-Processing']['AccelY_mss'], h5Data['Sensor-Processing']['AccelZ_mss']]) 167 | oData['wB_I_rps'] = np.array([h5Data['Sensor-Processing']['GyroX_rads'], h5Data['Sensor-Processing']['GyroY_rads'], h5Data['Sensor-Processing']['GyroZ_rads']]) 168 | oData['sB_L_rad'] = np.array([h5Data['Sensor-Processing']['Roll_rad'], h5Data['Sensor-Processing']['Pitch_rad'], h5Data['Sensor-Processing']['Heading_rad']]) 169 | 170 | # Mission 171 | oData['socEngage'] = h5Data['Mission']['socEngage'] 172 | oData['ctrlSel'] = h5Data['Mission']['ctrlSel'] 173 | oData['testID'] = h5Data['Mission']['testID'] 174 | oData['exciteEngage'] = h5Data['Mission']['excitEngage'] 175 | 176 | # Power 177 | oData['pwrFmu_V'] = h5Data['Sensors']['Fmu']['Voltage']['Input_V'] 178 | oData['pwrFmuReg_V'] = h5Data['Sensors']['Fmu']['Voltage']['Regulated_V'] 179 | 180 | if 'currAvionics' in h5Data['Sensors']['Power']: 181 | oData['pwrAvionics_A'] = h5Data['Sensors']['Power']['currAvionics']['CalibratedValue'] 182 | oData['pwrAvionics_V'] = h5Data['Sensors']['Power']['voltAvionics']['CalibratedValue'] 183 | 184 | if 'currPropLeft' in h5Data['Sensors']['Power']: 185 | oData['pwrPropLeft_A'] = h5Data['Sensors']['Power']['currPropLeft']['CalibratedValue'] 186 | oData['pwrPropLeft_V'] = h5Data['Sensors']['Power']['voltPropLeft']['CalibratedValue'] 187 | 188 | if 'currPropRight' in h5Data['Sensors']['Power']: 189 | oData['pwrPropRight_A'] = h5Data['Sensors']['Power']['currPropRight']['CalibratedValue'] 190 | oData['pwrPropRight_V'] = h5Data['Sensors']['Power']['voltPropRight']['CalibratedValue'] 191 | 192 | 193 | # Excitations 194 | oData['Excitation'] = {} 195 | for k, v in h5Data['Excitation'].items(): 196 | for sigName, sigVal in v.items(): 197 | if sigName in oData['Excitation']: 198 | oData['Excitation'][sigName] += sigVal 199 | else: 200 | oData['Excitation'][sigName] = sigVal 201 | 202 | # Make sure the base values are available from the excitation 203 | oData['Control'] = {} 204 | for sigName in oData['Excitation'].keys(): 205 | if sigName not in oData['Control']: 206 | oData['Control'][sigName] = h5Data['Control'][sigName] 207 | 208 | return oData 209 | 210 | 211 | #%% HDF5 Read 212 | def Log_JSBSim(filename): 213 | 214 | simData = Load_CSV(filename) 215 | oData = OpenData_RAPTRS(simData) 216 | 217 | return oData, simData 218 | 219 | 220 | #%% Read JSBSim CSV log 221 | def Load_CSV(filename): 222 | 223 | simData = {} 224 | 225 | with open(filename, 'r') as csvFile: 226 | reader = csv.reader(csvFile) 227 | count = 0 228 | 229 | for row in reader: 230 | if count == 0: 231 | names = row 232 | for i in range(0, len(row)): 233 | simData[names[i]] = np.array([]) 234 | else: 235 | for i in range(0, len(row)): 236 | simData[names[i]] = np.append(simData[names[i]], float(row[i])) 237 | 238 | count += 1 239 | 240 | csvFile.close() 241 | 242 | return simData 243 | 244 | #%% 245 | def OpenData_JSBSim(simData, oData = {}): 246 | 247 | # Time 248 | oData['time_s'] = simData['Time'] # !!Unsure on units!! 249 | 250 | # GPS 251 | oData['alt_true_m'] = simData['/fdm/jsbsim/sensor/gps/alt_true_m'] 252 | oData['lat_true_rad'] = simData['/fdm/jsbsim/sensor/gps/lat_true_rad'] 253 | oData['long_true_rad'] = simData['/fdm/jsbsim/sensor/gps/long_true_rad'] 254 | oData['v_true_mps'] = np.array([simData['/fdm/jsbsim/sensor/gps/vEast_true_mps'], simData['/fdm/jsbsim/sensor/gps/vNorth_true_mps'], simData['/fdm/jsbsim/sensor/gps/vDown_true_mps']]) 255 | 256 | # IMU 257 | oData['accel_true_fps2'] = np.array([simData['/fdm/jsbsim/sensor/imu/accelX_true_fps2'], simData['/fdm/jsbsim/sensor/imu/accelY_true_fps2'], simData['/fdm/jsbsim/sensor/imu/accelZ_true_fps2']]) 258 | #oData['gyro_true_rps'] = np.array([simData['/fdm/jsbsim/sensor/imu/gyroX_true_rps'], simData['/fdm/jsbsim/sensor/imu/gyroY_true_rps'], simData['/fdm/jsbsim/sensor/imu/gyroZ_true_rps']]) 259 | 260 | # Pitot 261 | oData['presStatic_true_pa'] = simData['/fdm/jsbsim/sensor/pitot/presStatic_true_Pa'] 262 | oData['presTip_true_pa'] = simData['/fdm/jsbsim/sensor/pitot/presTip_true_Pa'] 263 | oData['temp_true_C'] = simData['/fdm/jsbsim/sensor/pitot/temp_true_C'] 264 | 265 | # Altitude 266 | oData['alt_AGL_ft'] = simData['Altitude AGL (ft)'] 267 | oData['alt_ASL_ft'] = simData['Altitude ASL (ft)'] 268 | 269 | # Alpha and Beta 270 | oData['alpha_deg'] = simData['Alpha (deg)'] 271 | oData['beta_deg'] = simData['Beta (deg)'] 272 | 273 | # Body Acceleration 274 | oData['accel_body_units'] = np.array([simData['BodyAccel_X'], simData['BodyAccel_Y'], simData['BodyAccel_Z']]) # !!Unsure on units!! 275 | 276 | # Moments of Inertia 277 | oData['I_xx_units'] = simData['I_{xx}'] # !!Unsure on units!! 278 | oData['I_xy_units'] = simData['I_{xy}'] # !!Unsure on units!! 279 | oData['I_xz_units'] = simData['I_{xz}'] # !!Unsure on units!! 280 | oData['I_yx_units'] = simData['I_{yx}'] # !!Unsure on units!! 281 | oData['I_yy_units'] = simData['I_{yy}'] # !!Unsure on units!! 282 | oData['I_yz_units'] = simData['I_{yz}'] # !!Unsure on units!! 283 | oData['I_zx_units'] = simData['I_{zx}'] # !!Unsure on units!! 284 | oData['I_zy_units'] = simData['I_{zy}'] # !!Unsure on units!! 285 | oData['I_zz_units'] = simData['I_{zz}'] # !!Unsure on units!! 286 | 287 | # P, Q, R 288 | oData['p_deg/s'] = simData['P (deg/s)'] 289 | oData['q_deg/s'] = simData['Q (deg/s)'] 290 | oData['r_deg/s'] = simData['R (deg/s)'] 291 | 292 | # Pdot, Qdot, Rdot 293 | oData['pdot_deg/s2'] = simData['P dot (deg/s^2)'] 294 | oData['qdot_deg/s2'] = simData['Q dot (deg/s^2)'] 295 | oData['rdot_deg/s2'] = simData['R dot (deg/s^2)'] 296 | 297 | # Qbar 298 | oData['qbar_psf'] = simData['q bar (psf)'] 299 | 300 | # Wind 301 | oData['wind_fps'] = np.array([simData['Wind V_{East} (ft/s)'],simData['Wind V_{North} (ft/s)'],simData['Wind V_{Down} (ft/s)']]) 302 | 303 | return(oData) 304 | -------------------------------------------------------------------------------- /Examples/HuginnRtsm_Disturb.py: -------------------------------------------------------------------------------- 1 | """ 2 | University of Minnesota 3 | Aerospace Engineering and Mechanics - UAV Lab 4 | Copyright 2019 Regents of the University of Minnesota 5 | See: LICENSE.md for complete license details 6 | 7 | Author: Chris Regan 8 | 9 | Analysis for Huginn (mAEWing2) FLT 03-06 10 | """ 11 | 12 | #%% 13 | # Import Libraries 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | 17 | # Hack to allow loading the Core package 18 | if __name__ == "__main__" and __package__ is None: 19 | from sys import path, argv 20 | from os.path import dirname, abspath, join 21 | 22 | path.insert(0, abspath(join(dirname(argv[0]), ".."))) 23 | path.insert(0, abspath(join(dirname(argv[0]), "..", 'Core'))) 24 | 25 | del path, argv, dirname, abspath, join 26 | 27 | from Core import Loader 28 | from Core import OpenData 29 | 30 | 31 | # Constants 32 | hz2rps = 2 * np.pi 33 | rps2hz = 1 / hz2rps 34 | 35 | 36 | #%% File Lists 37 | import os.path as path 38 | 39 | pathBase = path.join('/home', 'rega0051', 'FlightArchive', 'Huginn') 40 | #pathBase = path.join('G:', 'Shared drives', 'UAVLab', 'Flight Data', 'Huginn') 41 | #pathBase = path.join('D:/', 'Huginn') 42 | 43 | fileList = {} 44 | flt = 'FLT03' 45 | fileList[flt] = {} 46 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 47 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 48 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 49 | 50 | flt = 'FLT04' 51 | fileList[flt] = {} 52 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 53 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 54 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 55 | 56 | flt = 'FLT05' 57 | fileList[flt] = {} 58 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 59 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 60 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 61 | 62 | flt = 'FLT06' 63 | fileList[flt] = {} 64 | fileList[flt]['log'] = path.join(pathBase, 'Huginn' + flt, 'Huginn' + flt + '.h5') 65 | fileList[flt]['config'] = path.join(pathBase, 'Huginn' + flt, 'huginn.json') 66 | fileList[flt]['def'] = path.join(pathBase, 'Huginn' + flt, 'huginn_def.json') 67 | 68 | 69 | #%% 70 | from Core import FreqTrans 71 | 72 | rtsmSegList = [ 73 | {'flt': 'FLT03', 'seg': ('time_us', [693458513 , 705458513], 'FLT03 - 23 m/s'), 'fmt': 'k'}, # 23 m/s 74 | {'flt': 'FLT03', 'seg': ('time_us', [865877709 , 877877709], 'FLT03 - 20 m/s'), 'fmt': 'k'}, # 20 m/s 75 | 76 | {'flt': 'FLT04', 'seg': ('time_us', [884683583, 896683583], 'FLT04 - 26 m/s'), 'fmt': 'k'}, # 26 m/s 77 | {'flt': 'FLT04', 'seg': ('time_us', [998733748, 1010733748], 'FLT04 - 20 m/s'), 'fmt': 'k'}, # 20 m/s 78 | 79 | {'flt': 'FLT06', 'seg': ('time_us', [1122539650, 1134539650], 'FLT06 - 23 m/s'), 'fmt': 'k'}, # 23 m/s 80 | 81 | {'flt': 'FLT05', 'seg': ('time_us', [582408497, 594408497], 'FLT05 - 26 m/s'), 'fmt': 'b'}, # 26 m/s 82 | {'flt': 'FLT05', 'seg': ('time_us', [799488311, 811488311], 'FLT05 - 29 m/s'), 'fmt': 'g'}, # 29 m/s 83 | 84 | {'flt': 'FLT06', 'seg': ('time_us', [955822061, 967822061], 'FLT06 - 32 m/s'), 'fmt': 'm'}, # 32 m/s 85 | ] 86 | 87 | 88 | oDataSegs = [] 89 | for rtsmSeg in rtsmSegList: 90 | fltNum = rtsmSeg['flt'] 91 | 92 | fileLog = fileList[fltNum]['log'] 93 | fileConfig = fileList[fltNum]['config'] 94 | 95 | # Load 96 | h5Data = Loader.Load_h5(fileLog) # RAPTRS log data as hdf5 97 | sysConfig = Loader.JsonRead(fileConfig) 98 | oData = Loader.OpenData_RAPTRS(h5Data, sysConfig) 99 | 100 | # Create Signal for Bending Measurement 101 | aZ = np.array([ 102 | oData['aCenterFwdIMU_IMU_mps2'][2] - oData['aCenterFwdIMU_IMU_mps2'][2][0], 103 | oData['aCenterAftIMU_IMU_mps2'][2] - oData['aCenterAftIMU_IMU_mps2'][2][0], 104 | oData['aLeftMidIMU_IMU_mps2'][2] - oData['aLeftMidIMU_IMU_mps2'][2][0], 105 | oData['aLeftFwdIMU_IMU_mps2'][2] - oData['aLeftFwdIMU_IMU_mps2'][2][0], 106 | oData['aLeftAftIMU_IMU_mps2'][2] - oData['aLeftAftIMU_IMU_mps2'][2][0], 107 | oData['aRightMidIMU_IMU_mps2'][2] - oData['aRightMidIMU_IMU_mps2'][2][0], 108 | oData['aRightFwdIMU_IMU_mps2'][2] - oData['aRightFwdIMU_IMU_mps2'][2][0], 109 | oData['aRightAftIMU_IMU_mps2'][2] - oData['aRightAftIMU_IMU_mps2'][2][0] 110 | ]) 111 | 112 | aCoefEta1 = np.array([456.1, -565.1, -289.9, 605.4, 472.4, -292, 605.6, 472.2]) * 0.3048 113 | measEta1 = aCoefEta1 @ (aZ - np.mean(aZ, axis = 0)) * 1/50 * 1e-3 114 | 115 | 116 | aCoefEta1dt = np.array([2.956, -2.998, -1.141, 5.974, 5.349, -1.149, 5.974, 5.348]) * 0.3048 117 | measEta1dt = aCoefEta1dt @ (aZ - np.mean(aZ, axis = 0)) * 1e-3 118 | 119 | # Added signals 120 | oData['cmdRoll_FF'] = h5Data['Control']['cmdRoll_PID_rpsFF'] 121 | oData['cmdRoll_FB'] = h5Data['Control']['cmdRoll_PID_rpsFB'] 122 | oData['cmdPitch_FF'] = h5Data['Control']['cmdPitch_PID_rpsFF'] 123 | oData['cmdPitch_FB'] = h5Data['Control']['cmdPitch_PID_rpsFB'] 124 | oData['cmdBend_FF'] = h5Data['Control']['refBend_nd'] # h5Data['Excitation']['Bend']['cmdBend_nd'] 125 | oData['cmdBendDt_FB'] = measEta1dt 126 | oData['cmdBend_FB'] = measEta1 127 | 128 | # Segments 129 | seg = OpenData.Segment(oData, rtsmSeg['seg']) 130 | oDataSegs.append(seg) 131 | 132 | # plt.plot(seg['time_s'], seg['Control']['cmdBend_nd'], seg['time_s'], seg['cmdBendDt_FB'], seg['time_s'], seg['cmdBend_FB']) 133 | 134 | 135 | #%% 136 | 137 | sigExcList = ['cmdRoll_rps', 'cmdPitch_rps', 'cmdBend_nd'] 138 | sigFbList = ['cmdRoll_FB', 'cmdPitch_FB', 'cmdBend_FB'] 139 | sigFfList = ['cmdRoll_FF', 'cmdPitch_FF', 'cmdBend_FF'] 140 | 141 | freqExc_rps = [] 142 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_1']['Frequency'])) 143 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_2']['Frequency'])) 144 | freqExc_rps.append( np.array(sysConfig['Excitation']['OMS_RTSM_3']['Frequency'])) 145 | 146 | vCmdList = [] 147 | vExcList = [] 148 | vFbList = [] 149 | vFfList = [] 150 | for iSeg, seg in enumerate(oDataSegs): 151 | vCmd = np.zeros((len(sigExcList), len(seg['time_s']))) 152 | vExc = np.zeros((len(sigExcList), len(seg['time_s']))) 153 | vFb = np.zeros((len(sigExcList), len(seg['time_s']))) 154 | vFf = np.zeros((len(sigExcList), len(seg['time_s']))) 155 | 156 | for iSig, sigExc in enumerate(sigExcList): 157 | sigFb = sigFbList[iSig] 158 | sigFf = sigFfList[iSig] 159 | 160 | vCmd[iSig] = seg['Control'][sigExc] 161 | vExc[iSig] = seg['Excitation'][sigExc] 162 | vFb[iSig][1:-1] = seg[sigFb][0:-2] # Shift the time of the output into next frame 163 | vFf[iSig] = seg[sigFf] 164 | 165 | vCmdList.append(vCmd) 166 | vExcList.append(vExc) 167 | vFbList.append(vFb) 168 | vFfList.append(vFf) 169 | 170 | plt.plot(oDataSegs[iSeg]['time_s'], oDataSegs[iSeg]['vIas_mps']) 171 | 172 | # plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][0]) 173 | # plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][1]) 174 | # plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][2]) 175 | # plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][0]) 176 | # plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][1]) 177 | # plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][2]) 178 | 179 | 180 | #%% Estimate the frequency response function 181 | # Define the excitation frequencies 182 | freqRate_hz = 50 183 | freqRate_rps = freqRate_hz * hz2rps 184 | optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.2), detrendType = 'Linear') 185 | 186 | # Excited Frequencies per input channel 187 | optSpec.freq = np.asarray(freqExc_rps) 188 | optSpec.freqInterp = np.sort(optSpec.freq.flatten()) 189 | 190 | # Null Frequencies 191 | freqGap_rps = optSpec.freq.flatten()[0:-1] + 0.5 * np.diff(optSpec.freq.flatten()) 192 | optSpec.freqNull = freqGap_rps 193 | optSpec.freqNullInterp = True 194 | 195 | # FRF Estimate 196 | T = [] 197 | TUnc = [] 198 | C = [] 199 | for iSeg, seg in enumerate(oDataSegs): 200 | 201 | freq_rps, Teb, Ceb, Pee, Pbb, Peb, TebUnc, Pee_N, Pbb_N = FreqTrans.FreqRespFuncEstNoise(vExcList[iSeg], vFbList[iSeg], optSpec) 202 | _ , Tev, Cev, _ , Pvv, Pev, TevUnc, _ , Pvv_N = FreqTrans.FreqRespFuncEstNoise(vExcList[iSeg], vCmdList[iSeg], optSpec) 203 | 204 | freq_hz = freq_rps * rps2hz 205 | 206 | # Form the Frequency Response 207 | T_seg = np.empty_like(Tev) 208 | TUnc_seg = np.empty_like(Tev) 209 | 210 | for i in range(T_seg.shape[-1]): 211 | T_seg[...,i] = Teb[...,i] @ np.linalg.inv(Tev[...,i]) 212 | TUnc_seg[...,i] = TebUnc[...,i] @ np.linalg.inv(Tev[...,i]) 213 | 214 | 215 | T.append( T_seg ) 216 | TUnc.append( np.abs(TUnc_seg) ) 217 | 218 | # C.append(Cev) 219 | C.append(Ceb) 220 | 221 | 222 | T_InputNames = sigExcList 223 | T_OutputNames = sigFbList 224 | 225 | # Compute Gain, Phase, Crit Distance 226 | gain_mag = [] 227 | gainUnc_mag = [] 228 | phase_deg = [] 229 | rCritNom_mag = [] 230 | rCritUnc_mag = [] 231 | rCrit_mag = [] 232 | sNom = [] 233 | sUnc = [] 234 | for iSeg in range(0, len(oDataSegs)): 235 | 236 | gainElem_mag, gainElemUnc_mag, gainElemDiff_mag = FreqTrans.DistCritCirc(T[iSeg], TUnc[iSeg], pCrit = 0+0j, typeNorm = 'RSS') 237 | 238 | gain_mag.append(gainElem_mag) 239 | gainUnc_mag.append(gainElemUnc_mag) 240 | 241 | phase_deg.append(FreqTrans.Phase(T[iSeg], phaseUnit = 'deg', unwrap = True)) 242 | 243 | nom_mag, unc_mag, diff_mag = FreqTrans.DistCritCirc(T[iSeg], TUnc[iSeg], pCrit = -1+0j, typeNorm = 'RSS') 244 | 245 | rCritNom_mag.append(nom_mag) 246 | rCritUnc_mag.append(unc_mag) 247 | rCrit_mag.append(diff_mag) 248 | 249 | sNom_seg, sUnc_seg = FreqTrans.Sigma(T[iSeg], TUnc[iSeg]) # Singular Value Decomp, U @ S @ Vh == T[...,i] 250 | sNom.append(sNom_seg) 251 | sUnc.append(sUnc_seg) 252 | 253 | 254 | #%% Spectrograms 255 | if False: 256 | #%% 257 | iSgnlExc = 0 258 | iSgnlOut = 0 259 | 260 | freqRate_rps = 50 * hz2rps 261 | optSpec = FreqTrans.OptSpect(dftType = 'dftmat', freq = freqExc_rps[iSgnlExc], freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.0), detrendType = 'Linear') 262 | optSpecN = FreqTrans.OptSpect(dftType = 'dftmat', freq = freqGap_rps, freqRate = freqRate_rps, smooth = ('box', 3), winType = ('tukey', 0.0), detrendType = 'Linear') 263 | 264 | 265 | for iSeg in range(0, len(oDataSegs)): 266 | t = oDataSegs[iSeg]['time_s'] 267 | x = vExcList[iSeg][iSgnlExc] 268 | y = vFbList[iSeg][iSgnlOut] 269 | 270 | # Number of time segments and length of overlap, units of samples 271 | #lenSeg = 2**6 - 1 272 | lenSeg = int(1.0 * optSpec.freqRate * rps2hz) 273 | lenOverlap = 1 274 | 275 | # Compute Spectrum over time 276 | tSpecY_s, freqSpecY_rps, P_Y_mag = FreqTrans.SpectTime(t, y, lenSeg, lenOverlap, optSpec) 277 | tSpecN_s, freqSpecN_rps, P_N_mag = FreqTrans.SpectTime(t, y, lenSeg, lenOverlap, optSpecN) 278 | 279 | # Plot the Spectrogram 280 | fig = FreqTrans.Spectogram(tSpecY_s, freqSpecY_rps * rps2hz, 20 * np.log10(P_Y_mag)) 281 | fig.suptitle(oDataSegs[iSeg]['Desc'] + ': Spectrogram - ' + sigFbList[iSgnlOut]) 282 | 283 | fig = FreqTrans.Spectogram(tSpecN_s, freqSpecN_rps * rps2hz, 20 * np.log10(P_N_mag)) 284 | fig.suptitle(oDataSegs[iSeg]['Desc'] + ': Spectrogram Null - ' + sigFbList[iSgnlOut]) 285 | 286 | 287 | #%% Sigma Plot 288 | fig = None 289 | for iSeg in range(0, len(oDataSegs)): 290 | Cmin = np.min(np.min(C[iSeg], axis = 0), axis = 0) 291 | sNomMin = np.min(sNom[iSeg], axis=0) 292 | sCritMin = np.min(sUnc[iSeg], axis=0) 293 | sNomMinErr = sNomMin - sCritMin 294 | 295 | fig = FreqTrans.PlotSigma(freq_hz[0], sNomMin, err = sNomMinErr, coher_nd = Cmin, fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 296 | 297 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fmt = '--r', fig = fig) 298 | 299 | ax = fig.get_axes() 300 | ax[0].set_xlim(0, 10) 301 | ax[0].set_ylim(0, 1) 302 | 303 | 304 | #%% Disk Margin Plots 305 | inPlot = sigExcList # Elements of sigExcList 306 | outPlot = sigFbList # Elements of sigFbList 307 | 308 | if False: 309 | #%% 310 | for iOut, outName in enumerate(outPlot): 311 | for iIn, inName in enumerate(inPlot): 312 | 313 | fig = None 314 | for iSeg in range(0, len(oDataSegs)): 315 | fig = FreqTrans.PlotSigma(freq_hz[0], rCritNom_mag[iSeg][iOut, iIn], err = rCritUnc_mag[iSeg][iOut, iIn], coher_nd = C[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 316 | 317 | fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fig = fig, fmt = 'r--', label = 'Critical Limit') 318 | fig.suptitle(inName + ' to ' + outName, size=20) 319 | 320 | ax = fig.get_axes() 321 | ax[0].set_ylim(0, 2) 322 | 323 | 324 | #%% Nyquist Plots 325 | if False: 326 | #%% 327 | for iOut, outName in enumerate(outPlot): 328 | for iIn, inName in enumerate(inPlot): 329 | 330 | fig = None 331 | for iSeg in range(0, len(oDataSegs)): 332 | fig = FreqTrans.PlotNyquist(T[iSeg][iOut, iIn], TUnc[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*', label = oDataSegs[iSeg]['Desc']) 333 | 334 | fig = FreqTrans.PlotNyquist(np.asarray([-1+ 0j]), TUnc = np.asarray([0.4 + 0.4j]), fig = fig, fmt = '*r', label = 'Critical Region') 335 | fig.suptitle(inName + ' to ' + outName, size=20) 336 | 337 | ax = fig.get_axes() 338 | ax[0].set_xlim(-3, 1) 339 | ax[0].set_ylim(-2, 2) 340 | 341 | 342 | #%% Bode Plots 343 | if False: 344 | #%% 345 | for iOut, outName in enumerate(outPlot): 346 | for iIn, inName in enumerate(inPlot): 347 | 348 | fig = None 349 | for iSeg in range(0, len(oDataSegs)): 350 | # fig = FreqTrans.PlotBode(freq_hz[0], gain_mag[iSeg][iOut, iIn], phase_deg[iSeg][iOut, iIn], C[iSeg][iOut, iIn], gainUnc_mag = gainUnc_mag[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 351 | fig = FreqTrans.PlotBode(freq_hz[0], gain_mag[iSeg][iOut, iIn], phase_deg[iSeg][iOut, iIn], C[iSeg][iOut, iIn], fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc']) 352 | 353 | fig.suptitle(inName + ' to ' + outName, size=20) 354 | --------------------------------------------------------------------------------