Skip to content

Commit faea02b

Browse files
teubertteubert
authored
Merge pull request #12 from nasa/develop
Bugfixes and Particle Filter
2 parents c77a4d2 + 87fbd56 commit faea02b

40 files changed

+3129
-494
lines changed

‎.gitignore

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
2+
build/
3+
4+
bin/

‎CMakeLists.txt

+5-5
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8)
22

33
project(GSAP)
44

5-
set(CMAKE_BINARY_DIR ${CMAKE_SOURCE_DIR}/bin/)
5+
set(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/bin/)
66
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
77
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
88
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
@@ -88,7 +88,7 @@ else()
8888
endif()
8989

9090
#Libraries
91-
add_subdirectory(${CMAKE_SOURCE_DIR}/support/)
92-
add_subdirectory(${CMAKE_SOURCE_DIR}/framework/)
93-
add_subdirectory(${CMAKE_SOURCE_DIR}/Test/)
94-
add_subdirectory(${CMAKE_SOURCE_DIR}/example/)
91+
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/support/)
92+
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/framework/)
93+
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/Test/)
94+
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/example/)

‎Test/BenchmarkPrognoser/BatteryPlayback.txt

+101
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
Timestamp power voltage temperature
2+
0.00 8.00 20.00 4.00
3+
1.00 8.00 20.00 3.96
4+
2.00 8.00 20.00 3.93
5+
3.00 8.00 20.00 3.90
6+
4.00 8.00 20.00 3.88
7+
5.00 8.00 20.00 3.86
8+
6.00 8.00 20.00 3.84
9+
7.00 8.00 20.00 3.82
10+
8.00 8.00 20.00 3.81
11+
9.00 8.00 20.00 3.80
12+
10.00 8.00 20.00 3.79
13+
11.00 8.00 20.00 3.78
14+
12.00 8.00 20.00 3.78
15+
13.00 8.00 20.00 3.77
16+
14.00 8.00 20.00 3.76
17+
15.00 8.00 20.00 3.76
18+
16.00 8.00 20.00 3.76
19+
17.00 8.00 20.00 3.75
20+
18.00 8.00 20.00 3.75
21+
19.00 8.00 20.00 3.75
22+
20.00 8.00 20.00 3.74
23+
21.00 8.00 20.00 3.74
24+
22.00 8.00 20.00 3.74
25+
23.00 8.00 20.00 3.74
26+
24.00 8.00 20.00 3.74
27+
25.00 8.00 20.00 3.74
28+
26.00 8.00 20.00 3.74
29+
27.00 8.00 20.00 3.73
30+
28.00 8.00 20.00 3.73
31+
29.00 8.00 20.00 3.73
32+
30.00 8.00 20.00 3.73
33+
31.00 8.00 20.00 3.73
34+
32.00 8.00 20.00 3.73
35+
33.00 8.00 20.00 3.73
36+
34.00 8.00 20.00 3.73
37+
35.00 8.00 20.00 3.73
38+
36.00 8.00 20.00 3.73
39+
37.00 8.00 20.00 3.73
40+
38.00 8.00 20.00 3.73
41+
39.00 8.00 20.00 3.73
42+
40.00 8.00 20.00 3.73
43+
41.00 8.00 20.00 3.73
44+
42.00 8.00 20.00 3.72
45+
43.00 8.00 20.00 3.72
46+
44.00 8.00 20.00 3.72
47+
45.00 8.00 20.00 3.72
48+
46.00 8.00 20.00 3.72
49+
47.00 8.00 20.00 3.72
50+
48.00 8.00 20.00 3.72
51+
49.00 8.00 20.00 3.72
52+
50.00 8.00 20.00 3.72
53+
51.00 8.00 20.00 3.72
54+
52.00 8.00 20.00 3.72
55+
53.00 8.00 20.00 3.72
56+
54.00 8.00 20.00 3.72
57+
55.00 8.00 20.00 3.72
58+
56.00 8.00 20.00 3.72
59+
57.00 8.00 20.00 3.72
60+
58.00 8.00 20.00 3.72
61+
59.00 8.00 20.00 3.72
62+
60.00 8.00 20.00 3.72
63+
61.00 8.00 20.00 3.72
64+
62.00 8.00 20.00 3.72
65+
63.00 8.00 20.00 3.72
66+
64.00 8.00 20.00 3.72
67+
65.00 8.00 20.00 3.72
68+
66.00 8.00 20.00 3.72
69+
67.00 8.00 20.00 3.72
70+
68.00 8.00 20.00 3.72
71+
69.00 8.00 20.00 3.72
72+
70.00 8.00 20.00 3.72
73+
71.00 8.00 20.00 3.72
74+
72.00 8.00 20.00 3.72
75+
73.00 8.00 20.00 3.71
76+
74.00 8.00 20.00 3.71
77+
75.00 8.00 20.00 3.71
78+
76.00 8.00 20.00 3.71
79+
77.00 8.00 20.00 3.71
80+
78.00 8.00 20.00 3.71
81+
79.00 8.00 20.00 3.71
82+
80.00 8.00 20.00 3.71
83+
81.00 8.00 20.00 3.71
84+
82.00 8.00 20.00 3.71
85+
83.00 8.00 20.00 3.71
86+
84.00 8.00 20.00 3.71
87+
85.00 8.00 20.00 3.71
88+
86.00 8.00 20.00 3.71
89+
87.00 8.00 20.00 3.71
90+
88.00 8.00 20.00 3.71
91+
89.00 8.00 20.00 3.71
92+
90.00 8.00 20.00 3.71
93+
91.00 8.00 20.00 3.71
94+
92.00 8.00 20.00 3.71
95+
93.00 8.00 20.00 3.71
96+
94.00 8.00 20.00 3.71
97+
95.00 8.00 20.00 3.71
98+
96.00 8.00 20.00 3.71
99+
97.00 8.00 20.00 3.71
100+
98.00 8.00 20.00 3.71
101+
99.00 8.00 20.00 3.71

‎Test/BenchmarkPrognoser/BenchmarkPrognoser.cpp

+170
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,170 @@
1+
/** Model-based Prognoser - Body
2+
* @class BenchmarkPrognoser BenchmarkPrognoser.h Benchmark.h
3+
*
4+
* @brief Benchmark Prognoser Class
5+
*
6+
* BenchmarkPrognoser class.
7+
*
8+
* @author Micah Ricks
9+
* @version 0.1.0
10+
*
11+
*
12+
* Contact: Micah Ricks (mricks1@bulldogs.aamu.edu)
13+
* Created: January 31, 2017
14+
*
15+
* @copyright Copyright (c) 2017 United States Government as represented by
16+
* the Administrator of the National Aeronautics and Space Administration.
17+
* All Rights Reserved.
18+
*/
19+
20+
#include <stdio.h>
21+
#include <stdlib.h>
22+
#include <cstddef>
23+
24+
#include <memory>
25+
#include <vector>
26+
27+
#include "Benchmark.h"
28+
#include "SharedLib.h"
29+
#include "BenchmarkPrognoser.h"
30+
#include "ObserverFactory.h"
31+
#include "PredictorFactory.h"
32+
#include "PrognosticsModelFactory.h"
33+
#include "UData.h"
34+
#include "CommManager.h"
35+
#include "GSAPConfigMap.h"
36+
37+
namespace PCOE {
38+
// Configuration Keys
39+
const std::string MODEL_KEY = "model";
40+
const std::string OBSERVER_KEY = "observer";
41+
const std::string PREDICTOR_KEY = "predictor";
42+
const std::string EVENT_KEY = "Model.event";
43+
const std::string NUMSAMPLES_KEY = "Predictor.numSamples";
44+
const std::string HORIZON_KEY = "Predictor.horizon";
45+
const std::string PREDICTEDOUTPUTS_KEY = "Model.predictedOutputs";
46+
const std::string INPUTS_KEY = "inputs";
47+
const std::string OUTPUTS_KEY = "outputs";
48+
Benchmark benchmark1,benchmark2;
49+
unsigned long long const INIT_TIME=0;
50+
51+
BenchmarkPrognoser::BenchmarkPrognoser(GSAPConfigMap & configMap) : CommonPrognoser(configMap), initialized(false) {
52+
// Check for required config parameters
53+
configMap.checkRequiredParams({ MODEL_KEY,OBSERVER_KEY,PREDICTOR_KEY,EVENT_KEY,NUMSAMPLES_KEY,HORIZON_KEY,PREDICTEDOUTPUTS_KEY,INPUTS_KEY,OUTPUTS_KEY });
54+
/// TODO(CT): Move Model, Predictor subkeys into Model/Predictor constructor
55+
56+
// Create Model
57+
log.WriteLine(LOG_DEBUG, moduleName, "Creating Model");
58+
PrognosticsModelFactory & pProgModelFactory = PrognosticsModelFactory::instance();
59+
model = std::unique_ptr<PrognosticsModel>(pProgModelFactory.Create(configMap[MODEL_KEY][0], configMap));
60+
61+
// Create Observer
62+
log.WriteLine(LOG_DEBUG, moduleName, "Creating Observer");
63+
ObserverFactory & pObserverFactory = ObserverFactory::instance();
64+
observer = std::unique_ptr<Observer>(pObserverFactory.Create(configMap[OBSERVER_KEY][0], configMap));
65+
66+
// Create Predictor
67+
log.WriteLine(LOG_DEBUG, moduleName, "Creating Predictor");
68+
PredictorFactory & pPredictorFactory = PredictorFactory::instance();
69+
predictor = std::unique_ptr<Predictor>(pPredictorFactory.Create(configMap[PREDICTOR_KEY][0], configMap));
70+
71+
// Set model for observer and predictor
72+
observer->setModel(model.get());
73+
predictor->setModel(model.get());
74+
75+
// Set configuration parameters
76+
unsigned int numSamples = static_cast<unsigned int>(std::stoul(configMap[NUMSAMPLES_KEY][0]));
77+
unsigned int horizon = static_cast<unsigned int>(std::stoul(configMap[HORIZON_KEY][0]));
78+
std::string event = configMap[EVENT_KEY][0];
79+
std::vector<std::string> predictedOutputs = configMap[PREDICTEDOUTPUTS_KEY];
80+
81+
// Set inputs and outputs
82+
inputs = configMap[INPUTS_KEY];
83+
outputs = configMap[OUTPUTS_KEY];
84+
85+
// Create progdata
86+
results.setUncertainty(UType::Samples); // @todo(MD): do not force samples representation
87+
results.addEvent(event); // @todo(MD): do not assume only a single event
88+
results.addSystemTrajectories(predictedOutputs); // predicted outputs
89+
results.setPredictions(1, horizon); // interval, number of predictions
90+
results.setupOccurrence(numSamples);
91+
results.events[event].timeOfEvent.npoints(numSamples);
92+
results.sysTrajectories.setNSamples(numSamples);
93+
}
94+
95+
void BenchmarkPrognoser::step() {
96+
97+
//init time in nanoseconds
98+
if(benchmark2.begin != INIT_TIME)
99+
{
100+
benchmark2.nanosecondsEnd();
101+
benchmark2.findElapsedTime();
102+
103+
}
104+
benchmark1.nanosecondsBegin();
105+
106+
static double initialTime = comm.getValue(outputs[0]).getTime() / 1.0e3;
107+
108+
// Get new relative time (convert to seconds)
109+
// @todo(MD): Add config for time units so conversion is not hard-coded
110+
double newT = comm.getValue(outputs[0]).getTime() / 1.0e3 - initialTime;
111+
112+
// Fill in input and output data
113+
log.WriteLine(LOG_DEBUG, moduleName, "Getting data in step");
114+
std::vector<double> u(model->getNumInputs());
115+
std::vector<double> z(model->getNumOutputs());
116+
for (unsigned int i = 0; i < model->getNumInputs(); i++) {
117+
u[i] = comm.getValue(inputs[i]);
118+
}
119+
for (unsigned int i = 0; i < model->getNumOutputs(); i++) {
120+
z[i] = comm.getValue(outputs[i]);
121+
}
122+
123+
// If this is the first step, will want to initialize the observer and the predictor
124+
if (!initialized) {
125+
log.WriteLine(LOG_DEBUG, moduleName, "Initializing BenchmarkPrognoser");
126+
std::vector<double> x(model->getNumStates());
127+
model->initialize(x, u, z);
128+
observer->initialize(newT, x, u);
129+
initialized = true;
130+
lastTime = newT;
131+
} else {
132+
// If time has not advanced, skip this step
133+
if (newT <= lastTime) {
134+
log.WriteLine(LOG_TRACE, moduleName, "Skipping step because time did not advance.");
135+
136+
benchmark1.nanosecondsEnd();
137+
benchmark1.findElapsedTime();
138+
benchmark2.nanosecondsBegin();
139+
return;
140+
}
141+
142+
// Run observer
143+
log.WriteLine(LOG_DEBUG, moduleName, "Running Observer Step");
144+
observer->step(newT, u, z);
145+
log.WriteLine(LOG_DEBUG, moduleName, "Done Running Observer Step");
146+
147+
// Run predictor
148+
log.WriteLine(LOG_DEBUG, moduleName, "Running Prediction Step");
149+
// Set up state
150+
std::vector<UData> stateEst = observer->getStateEstimate();
151+
predictor->predict(newT, stateEst, results);
152+
log.WriteLine(LOG_DEBUG, moduleName, "Done Running Prediction Step");
153+
154+
// Set lastTime
155+
lastTime = newT;
156+
157+
}
158+
}
159+
160+
//destructor
161+
BenchmarkPrognoser::~BenchmarkPrognoser() {
162+
benchmark1.clearFile();
163+
benchmark1.printTemp();
164+
benchmark1.printScreen();
165+
benchmark2.printScreen();
166+
benchmark1.writeFile();
167+
benchmark2.writeFile();
168+
}
169+
170+
}

‎Test/BenchmarkPrognoser/BenchmarkPrognoser.h

+54
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
/** Benchmark Prognoser - Header
2+
* @class BenchmarkPrognoser BenchmarkPrognoser.h
3+
*
4+
* @brief Benchmark Prognoser Class
5+
*
6+
* BenchmarkPrognoser class.
7+
*
8+
* @author Micah Ricks
9+
* @version 0.1.0
10+
*
11+
*
12+
* Contact: Micah Ricks (mricks1@bulldogs.aamu.edu)
13+
* Created: March 16, 2016
14+
*
15+
* @copyright Copyright (c) 2017 United States Government as represented by
16+
* the Administrator of the National Aeronautics and Space Administration.
17+
* All Rights Reserved.
18+
*/
19+
20+
#ifndef PCOE_BENCHMARKPROGNOSER_H
21+
#define PCOE_BENCHMARKPROGNOSER_H
22+
23+
24+
#include "CommonPrognoser.h"
25+
#include "PrognosticsModel.h"
26+
#include "Observer.h"
27+
#include "Predictor.h"
28+
29+
namespace PCOE {
30+
class BenchmarkPrognoser : public CommonPrognoser
31+
{
32+
private:
33+
std::unique_ptr<PrognosticsModel> model;
34+
std::unique_ptr<Observer> observer;
35+
std::unique_ptr<Predictor> predictor;
36+
std::vector<std::string> inputs;
37+
std::vector<std::string> outputs;
38+
bool initialized;
39+
double firstTime;
40+
double lastTime;
41+
42+
public:
43+
/** @brief Benchmark Prognoser Constructor
44+
* @param config Map of config parameters from the prognoser config file
45+
*/
46+
BenchmarkPrognoser(GSAPConfigMap & config);
47+
~BenchmarkPrognoser(); //destructor
48+
void step();
49+
};
50+
51+
extern bool regModelProg;
52+
}
53+
54+
#endif // PCOE_BENCHMARKPROGNOSER_H

‎Test/BenchmarkPrognoser/CMakeLists.txt

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
set(HEADERS
2+
BenchmarkPrognoser.h
3+
)
4+
5+
set(SRCS
6+
BenchmarkPrognoser.cpp
7+
main.cpp
8+
)
9+
10+
include_directories(${CMAKE_SOURCE_DIR}/support/inc/)
11+
include_directories(${CMAKE_SOURCE_DIR}/framework/inc/)
12+
include_directories(${CMAKE_SOURCE_DIR}/test/inc/)
13+
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
14+
15+
link_libraries(testLib framework support)
16+
add_executable(BenchmarkPrognoser ${HEADERS} ${SRCS})
17+
18+
file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/BatteryPlayback.txt DESTINATION ${CMAKE_SOURCE_DIR}/bin/)

0 commit comments

Comments
 (0)