Skip to content
Snippets Groups Projects
Commit 0f03accb authored by Federico Lolli's avatar Federico Lolli
Browse files

[SFD] added SVM model and SFD architectures

parent 3b034804
Branches
No related tags found
No related merge requests found
......@@ -41,6 +41,8 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp
${SBS_BASE}/src/shared/algorithms/NAS/StateInitializer.cpp
${SBS_BASE}/src/shared/algorithms/SFD/SFDAscent.cpp
${SBS_BASE}/src/shared/algorithms/SFD/SFDDescent.cpp
# Debug
${SBS_BASE}/src/shared/utils/Debug.cpp
......
/* Copyright (c) 2023 Skyward Experimental Rocketry
* Author: Federico Lolli
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
// ======= Sensor Fault Detection Model (SFDAscent) =======
#pragma once
#include "SFDAscent.h"
#include <algorithms/FFT.h>
namespace Boardcore
{
SFDAscent::SFDAscent(const SFDAConfig& config) : svm(config.modelParameters) {}
SFDAscent::FeaturesVec SFDAscent::getFeatures(const VectorIn& input)
{
float delta, min, max, u, var, s2, m4, rfmean, rfvar;
VectorIn rfourier;
VectorIn data, x0 = VectorIn::Zero();
FeaturesVec features = FeaturesVec::Zero();
min = input.minCoeff();
max = input.maxCoeff();
delta = max - min;
for (int i = 0; i < lenChunk; i++)
data(i) = (input(i) - min) / (std::max(delta, 1e-25f) * 2) - 1;
u = data.mean();
x0 = data - u * VectorIn::Ones();
var = x0.squaredNorm() / lenChunk;
s2 = x0.pow(2).mean();
m4 = x0.pow(4).mean();
rfourier = FFT32::fft(data); // TODO: fix complex -> float
rfmean = rfourier.mean();
rfvar = (rfourier - rfmean * VectorIn::Ones()).squaredNorm() / lenChunk;
features(0) = delta;
features(1) = var;
features(2) = m4 / std::pow(s2, 2);
features(3) = data.pow(5).mean();
features(4) = rfvar;
features(5) = rfourier.cwiseAbs().sum();
return features;
}
bool SFDAscent::classify(const VectorIn& input)
{
FeaturesVec features = getFeatures(input);
float score = svm.score(features);
return score < 0;
}
} // namespace Boardcore
/* Copyright (c) 2023 Skyward Experimental Rocketry
* Author: Federico Lolli
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
// ======= Sensor Fault Detection Model (SFDAscent) =======
#pragma once
#include <models/SVM.h>
#include <Eigen/Core>
namespace Boardcore
{
class SFDAscent
{
public:
static constexpr int numFeatures = 6;
static constexpr int lenChunk = 32;
using SVM = SVM<numFeatures>;
using FeaturesVec = Eigen::Vector<float, numFeatures>;
using VectorIn = Eigen::Vector<float, lenChunk>;
struct SFDAConfig
{
SVM::SVMConfig modelParameters;
};
SFDAscent(const SFDAConfig& config);
bool classify(const VectorIn& input);
private:
SVM svm;
FeaturesVec getFeatures(const VectorIn& input);
};
} // namespace Boardcore
/* Copyright (c) 2023 Skyward Experimental Rocketry
* Author: Federico Lolli
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
// ======= Sensor Fault Detection Model (SFDAscent) =======
#pragma once
#include "SFDDescent.h"
namespace Boardcore
{
SFDDescent::SFDDescent(const SFDDConfig& config) : svm(config.modelParameters)
{
}
SFDDescent::FeaturesVec SFDDescent::getFeatures(const VectorIn& input)
{
float delta, min, max, u, s2, m3, m4, rms;
VectorIn rfourier;
VectorIn data, x0 = VectorIn::Zero();
FeaturesVec features = FeaturesVec::Zero();
min = input.minCoeff();
max = input.maxCoeff();
delta = max - min;
for (int i = 0; i < lenChunk; i++)
data(i) = (input(i) - min) / (std::max(delta, 1e-25f) * 2) - 1;
u = data.mean();
x0 = data - u * VectorIn::Ones();
s2 = x0.pow(2).mean();
m3 = x0.pow(3).mean();
m4 = x0.pow(4).mean();
rms = std::sqrt(s2);
features(0) = data.cwiseAbs().maxCoeff() / rms;
features(1) = delta;
features(2) = u;
features(3) = m3 / std::pow(s2, 1.5);
features(4) = m4 / std::pow(s2, 2);
return features;
}
bool SFDDescent::classify(const VectorIn& input)
{
FeaturesVec features = getFeatures(input);
float score = svm.score(features);
return score < 0;
}
} // namespace Boardcore
/* Copyright (c) 2023 Skyward Experimental Rocketry
* Author: Federico Lolli
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
// ======= Sensor Fault Detection Model (SFDDescent) =======
#pragma once
#include <models/SVM.h>
#include <Eigen/Core>
namespace Boardcore
{
class SFDDescent
{
public:
static constexpr int numFeatures = 5;
static constexpr int lenChunk = 32;
using SVM = SVM<numFeatures>;
using FeaturesVec = Eigen::Vector<float, numFeatures>;
using VectorIn = Eigen::Vector<float, lenChunk>;
struct SFDDConfig
{
SVM::SVMConfig modelParameters;
};
SFDDescent(const SFDDConfig& config);
bool classify(const VectorIn& input);
private:
SVM svm;
FeaturesVec getFeatures(const VectorIn& input);
};
} // namespace Boardcore
/* Copyright (c) 2023 Skyward Experimental Rocketry
* Author: Federico Lolli
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <Eigen/Dense>
namespace Boardcore
{
template <int D>
class SVM
{
public:
using VectorD = Eigen::Vector<float, D>;
struct SVMConfig
{
VectorD beta;
VectorD mu;
VectorD sigma;
float bias;
float scale;
};
SVM(const SVMConfig& config)
: beta(config.beta), mu(config.mu), sigma(config.sigma),
bias(config.bias), scale(config.scale)
{
}
float score(VectorD input)
{
VectorD x = input - mu;
x /= sigma;
return -((x / scale).dot(beta) + bias);
}
private:
VectorD beta;
VectorD mu;
VectorD sigma;
float bias;
float scale;
};
} // namespace Boardcore
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment