From b8c1b8b55e13df4deec6d4d10426a9b75a5cb7a6 Mon Sep 17 00:00:00 2001 From: O'Connor Date: Fri, 10 Aug 2018 15:14:29 -0700 Subject: [PATCH 01/11] Added EKF files during Chris's tutorials --- inc/ExtendedKalmanFilter.h | 9 +++++++++ src/Observers/ExtendedKalmanFilter.cpp | 7 +++++++ 2 files changed, 16 insertions(+) create mode 100644 inc/ExtendedKalmanFilter.h create mode 100644 src/Observers/ExtendedKalmanFilter.cpp diff --git a/inc/ExtendedKalmanFilter.h b/inc/ExtendedKalmanFilter.h new file mode 100644 index 0000000..71cd384 --- /dev/null +++ b/inc/ExtendedKalmanFilter.h @@ -0,0 +1,9 @@ +// Copyright (c) 2018 United States Government as represented by the +// Administrator of the National Aeronautics and Space Administration. +// All Rights Reserved. + +#ifndef PCOE_EXTENDEDKALMANFILTER_H +#define PCOE_EXTENDEDKALMANFILTER_H + + +#endif diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp new file mode 100644 index 0000000..d10ed72 --- /dev/null +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -0,0 +1,7 @@ +// Copyright (c) 2018 United States Government as represented by the +// Administrator of the National Aeronautics and Space Administration. +// All Rights Reserved. + +#include + +#include "ExtendedKalmanFilter.h" From 98ac9b0552e6dec818ea5397f5af0537233ff270 Mon Sep 17 00:00:00 2001 From: O'Connor Date: Fri, 10 Aug 2018 16:00:01 -0700 Subject: [PATCH 02/11] Updated makefile --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c0e255..ad325f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,6 +58,7 @@ set (HEADERS inc/DataPoints.h inc/DataStore.h inc/Datum.h + inc/ExtendedKalmanFilter.h inc/EventDrivenPrognoser.h inc/Exceptions.h inc/Factory.h @@ -125,6 +126,7 @@ set(SRCS src/Messages/Message.cpp src/Messages/MessageBus.cpp src/Messages/MessageId.cpp + src/Observers/ExtendedKalmanFilter.cpp src/Observers/EventDrivenObserver.cpp src/Observers/ParticleFilter.cpp src/Observers/UnscentedKalmanFilter.cpp From 42dbd1c694f0a649102f1678e541325d36cc6ed4 Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Mon, 13 Aug 2018 15:22:56 -0700 Subject: [PATCH 03/11] Matt spelled his name wrong --- inc/Observers/Observer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/inc/Observers/Observer.h b/inc/Observers/Observer.h index 1921a4e..0d12818 100644 --- a/inc/Observers/Observer.h +++ b/inc/Observers/Observer.h @@ -15,7 +15,7 @@ namespace PCOE { /** * Represents an object that observes a model's state. * - * @author Mattew Daigle + * @author Matthew Daigle * @author Jason Watkins * @since 1.0 **/ From 7606f8c7d55f1caac37d6ea9adbc2d82c856aceb Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Fri, 17 Aug 2018 11:37:20 -0700 Subject: [PATCH 04/11] EKF in progress - still figuring out Jacobians and inherited code --- inc/ExtendedKalmanFilter.h | 157 ++++++++++++++ src/Observers/ExtendedKalmanFilter.cpp | 280 +++++++++++++++++++++++++ 2 files changed, 437 insertions(+) diff --git a/inc/ExtendedKalmanFilter.h b/inc/ExtendedKalmanFilter.h index 71cd384..8e7f1b4 100644 --- a/inc/ExtendedKalmanFilter.h +++ b/inc/ExtendedKalmanFilter.h @@ -6,4 +6,161 @@ #define PCOE_EXTENDEDKALMANFILTER_H +#endif + +/** + Work in progress: Still deciding how best to deal with Jacobians +**/ + +#include +#include + +#include "Matrix.h" +#include "Observers/Observer.h" + +namespace PCOE { + class ConfigMap; + + /** + * Implements EKF state estimation algorithm for non-linear models. Or at least will by the time I'm done with it. + * + ** + *** Denotes a note to myself that must be dealt with + ** + * + * @author Molly O'Connor + * Cannibalized from UnscentedKalmanFilter.h by + * @author Matthew Daigle + * @author Jason Watkins + **/ + class ExtendedKalmanFilter final : public Observer { + private: + /** + * Constructs a new @{code ExtendedKalmanFilter} instance and + * initializes the model. This constructor is only intended to be used + * by other constructors in UKF to set up model-related parameters. + * + * @param m A valid pointer to a model on which state estimation will be + * performed. The UKF does not take ownership of the model. + **/ + explicit ExtendedKalmanFilter(const Model* m); + + public: + /** + * Constructs a new @{code ExtendedKalmanFilter} instance with the + * given model and covariance matrices. + * + * @param m A valid pointer to a model on which state estimation will be + * performed. The EKF does not take ownership of the model. + * @param Q Process noise covariance matrix + * @param R Sensor noise covariance matrix + **/ + ExtendedKalmanFilter(const Model* m, const Matrix Q, const Matrix R); + + /** + * Constructs a new @{code UnscentedKalmanFilter} instance with the + * given model and with covariance matrices read from the provided + * config. + * + * @param m A valid pointer to a model on which state estimation + * will be performed. The UKF does not take ownership of + * the model. + * @param config A configuration from which to read covariance matrices. + **/ + ExtendedKalmanFilter(const Model* m, const ConfigMap& config); + + /** + * Sets the initial model state and computes initial sigma points. + * + * @param t0 Initial time + * @param x0 Initial model state + * @param u0 Initial model input + **/ + void initialize(double t0, + const Model::state_type& x0, + const Model::input_type& u0) override; + + /** + * Performs a single state estimation with the given model inputs and + * outputs. + * + * @param t The time at which to make a prediction. + * @param u The model input vector at time @{code t}. + * @param z The model output vector at time @{code t}. + **/ + void step(double t, const Model::input_type& u, const Model::output_type& z) override; + + /** + * Computes Jacobian for provided matrix using definition of a derivative in the case that no Jacobian is provided by user. + * Epsilon should be either a default value or provided by user. + * + * + ** + *** Correct input parameters? + ** + * + * @param mx Mean vector (should be last updated -- k|k-1). + * @param u From model, current u + * + **/ + const Matrix getJacobian(const Model::state_type& mx, const Model::input_type& u); + + /** + * + ** + *** + **** What does this do? + *** + ** + * + * Returns the current mean state estimate of the observer. + * + * @return The last calculated state estimate. + **/ + inline const Model::state_type& getStateMean() const override { + return xEstimated; + } + + /** + * + * Returns the current state estimate of the observer, including + * uncertainty. + * + * @return The last calculated state estimate calcualted by the + * observer. + **/ + std::vector getStateEstimate() const; + + /** + * + ** + *** + **** what + *** + ** + * + * Returns the current mean output estimate of the observer. + * + * @return The last output estimate calcualted by the observer.. + **/ + inline const Model::output_type& getOutputMean() const override { + return zEstimated; + } + + /** + * Gets the state covariance matrix. + **/ + const Matrix& getStateCovariance() const { + return P; + } + + private: + Model::state_type xEstimated; + Model::output_type zEstimated; + Matrix Q; + Matrix R; + Matrix P; + }; +} + #endif diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp index d10ed72..ccb3460 100644 --- a/src/Observers/ExtendedKalmanFilter.cpp +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -5,3 +5,283 @@ #include #include "ExtendedKalmanFilter.h" + + +/** + Functions are still largely copied from UKF, header file being designed first and waiting on decision for how to deal with Jacobians. +**/ + +#include +#include +#include +#include + +#include "Exceptions.h" +#include "ConfigMap.h" +//#include "Observers/ExtendedKalmanFilter.h" +#include "ThreadSafeLog.h" +#include "UData.h" + +namespace PCOE { + const static Log& log = Log::Instance(); + + // Configuration Keys + const std::string Q_KEY = "Observer.Q"; + const std::string R_KEY = "Observer.R"; + + // Other string constants + const std::string MODULE_NAME = "OBS-EKF"; + + + ExtendedKalmanFilter::ExtendedKalmanFilter(const Model* m) : Observer(m) { + Expect(m != nullptr, "Invalid model"); + xEstimated = model->getStateVector(); + uPrev = model->getInputVector(); + zEstimated = model->getOutputVector(); + } + + ExtendedKalmanFilter::ExtendedKalmanFilter(const Model* m, Matrix q, Matrix r) + : ExtendedKalmanFilter(m) { + Expect(q.rows() == q.cols(), "q is not square"); + Expect(q.rows() == model->getStateSize(), "Size of q does not match model state size"); + Expect(r.rows() == r.cols(), "q is not square"); + Expect(r.rows() == model->getOutputSize(), "Size of r does not match model output size"); + + Q = std::move(q); + R = std::move(r); + } + + // ConfigMap-based Constructor + ExtendedKalmanFilter::ExtendedKalmanFilter(const Model* model, const ConfigMap& config) + : ExtendedKalmanFilter(model) { + requireKeys(config, {Q_KEY, R_KEY}); + + // Set Q + log.WriteLine(LOG_TRACE, MODULE_NAME, "Setting Q"); + auto& QValues = config.getVector(Q_KEY); + Require(std::abs(std::sqrt(QValues.size()) - std::floor(std::sqrt(QValues.size()))) < 1e-12, + "Q values can not describe a square matrix"); + auto qDim = static_cast(sqrt(QValues.size())); + Q.resize(qDim, qDim); + std::size_t qValueIndex = 0; + for (std::size_t row = 0; row < qDim; row++) { + for (std::size_t col = 0; col < qDim; col++) { + Q[row][col] = std::stod(QValues[qValueIndex]); + qValueIndex++; + } + } + + // Set R + log.WriteLine(LOG_TRACE, MODULE_NAME, "Setting R"); + auto& RValues = config.getVector(R_KEY); + Require(std::abs(std::sqrt(RValues.size()) - std::floor(std::sqrt(RValues.size()))) < 1e-12, + "R values can not describe a square matrix"); + auto rDim = static_cast(sqrt(RValues.size())); + R.resize(rDim, rDim); + std::size_t rValueIndex = 0; + for (std::size_t row = 0; row < rDim; row++) { + for (std::size_t col = 0; col < rDim; col++) { + R[row][col] = std::stod(RValues[rValueIndex]); + rValueIndex++; + } + } + + log.WriteLine(LOG_INFO, MODULE_NAME, "Created EKF"); + } + + + // Fix this for EKF + void ExtendedKalmanFilter::initialize(const double t0, + const Model::state_type& x0, + const Model::input_type& u0) { + log.WriteLine(LOG_DEBUG, MODULE_NAME, "Initializing"); + + // Initialize time, state, inputs + lastTime = t0; + xEstimated = x0; + uPrev = u0; + + // Initialize P + P = Q; + + // Compute initial Jacobians for current state estimate + //using definition of the derivative + //how do I figure out step size? Iterate? User input? Just pick something? + + // Compute corresponding output estimate + std::vector zeroNoiseZ(model->getOutputSize()); + zEstimated = model->outputEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); + + // Set initialized flag + initialized = true; + log.WriteLine(LOG_DEBUG, MODULE_NAME, "Initialize completed"); + } + + void ExtendedKalmanFilter::step(const double timestamp, + const Model::input_type& u, + const Model::output_type& z) { + log.WriteLine(LOG_DEBUG, MODULE_NAME, "Starting step"); + Expect(isInitialized(), "Not initialized"); + Expect(timestamp - lastTime > 0, "Time has not advanced"); + + // Update time + double dt = timestamp - lastTime; + lastTime = timestamp; + + std::size_t sigmaPointCount = sigmaX.M.cols(); + + // 1. Predict + log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - predict"); + + // Compute sigma points for current state estimate + computeSigmaPoints(xEstimated, Q, sigmaX); + + // Propagate sigma points through state equation + Matrix Xkk1(model->getStateSize(), sigmaPointCount); + std::vector zeroNoise(model->getStateSize()); + for (unsigned int i = 0; i < sigmaPointCount; i++) { + auto x = Model::state_type(static_cast>(sigmaX.M.col(i))); + x = model->stateEqn(timestamp, x, uPrev, zeroNoise, dt); + Xkk1.col(i, x.vec()); + } + + // Recombine weighted sigma points to produce predicted state and covariance + std::vector xkk1 = + static_cast>(Xkk1.weightedMean(Matrix(sigmaX.w))); + Matrix Pkk1 = Xkk1.weightedCovariance(Matrix(sigmaX.w), sigmaX.alpha, sigmaX.beta) + Q; + + // Propagate sigma points through output equation + Matrix Zkk1(model->getOutputSize(), sigmaPointCount); + for (unsigned int i = 0; i < sigmaPointCount; i++) { + auto zkk1 = + model->outputEqn(timestamp, + Model::state_type(static_cast>(Xkk1.col(i))), + u, + zeroNoise); + Zkk1.col(i, zkk1.vec()); + } + + // Recombine weighted sigma points to produce predicted measurement and covariance + std::vector zkk1 = + static_cast>(Zkk1.weightedMean(Matrix(sigmaX.w))); + Matrix Pzz = Zkk1.weightedCovariance(Matrix(sigmaX.w), sigmaX.alpha, sigmaX.beta) + R; + + // 2. Update + log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - update"); + + // Compute state-output cross-covariance matrix + Matrix Pxz(model->getStateSize(), model->getOutputSize()); + for (unsigned int i = 0; i < sigmaPointCount; i++) { + Matrix columnx(model->getStateSize(), 1); + Matrix columnz(model->getOutputSize(), 1); + columnx.col(0, Xkk1.col(i)); + columnz.col(0, Zkk1.col(i)); + + Matrix xkk1m(model->getStateSize(), 1); + Matrix zkk1m(model->getOutputSize(), 1); + xkk1m.col(0, xkk1); + zkk1m.col(0, zkk1); + + Matrix diffx = columnx - xkk1m; + Matrix diffzT = (columnz - zkk1m).transpose(); + Matrix temp = diffx * diffzT; + Pxz = Pxz + temp * sigmaX.w[i]; + } + + // Compute Kalman gain + Matrix Kk = Pxz * Pzz.inverse(); + + // Compute state estimate + Matrix xkk1m(model->getStateSize(), 1); + Matrix zkk1m(model->getOutputSize(), 1); + Matrix zm(model->getOutputSize(), 1); + xkk1m.col(0, xkk1); + zkk1m.col(0, zkk1); + zm.col(0, z.vec()); + Matrix xk1m = xkk1m + Kk * (zm - zkk1m); + xEstimated = Model::state_type(static_cast>(xk1m.col(0))); + + // Compute output estimate + std::vector zeroNoiseZ(model->getOutputSize()); + zEstimated = model->outputEqn(timestamp, xEstimated, u, zeroNoiseZ); + + // Compute covariance + P = Pkk1 - Kk * Pzz * Kk.transpose(); + + // Update uOld + uPrev = u; + } + + /*void ExtendedKalmanFilter::computeSigmaPoints(const Model::state_type& mx, + const Matrix& Pxx, + SigmaPoints& sigma) { + log.WriteLine(LOG_TRACE, MODULE_NAME, "Computing sigma points"); + + // Assumes that sigma points have been set up correctly within the constructor + auto stateSize = mx.size(); + auto sigmaPointCount = sigma.M.cols(); + + // First sigma point is the mean + for (unsigned int i = 0; i < stateSize; i++) { + sigma.M[i][0] = mx[i]; + } + + // Compute a matrix square root using Cholesky decomposition + Matrix nkPxx = Pxx; + for (unsigned int i = 0; i < nkPxx.rows(); i++) { + for (unsigned int j = 0; j < nkPxx.cols(); j++) { + nkPxx[i][j] *= (stateSize + sigma.kappa); + } + } + Matrix matrixSq = nkPxx.chol(); + + // For sigma points 2 to n+1, it is mx + ith column of matrix square root + for (unsigned int i = 0; i < stateSize; i++) { + // Set column + for (unsigned int j = 0; j < stateSize; j++) { + sigma.M[i][j + 1] = mx[i] + matrixSq[i][j]; + } + } + + // For sigma points n+2 to 2n+1, it is mx - ith column of matrix square root + for (unsigned int i = 0; i < stateSize; i++) { + // Set column + for (unsigned int j = 0; j < stateSize; j++) { + sigma.M[i][j + stateSize + 1] = mx[i] - matrixSq[i][j]; + } + } + + // w(1) is kappa/(n+kappa) + sigma.w[0] = sigma.kappa / (stateSize + sigma.kappa); + + // Rest of w are 0.5/(n+kappa) + for (unsigned int i = 1; i < sigmaPointCount; i++) { + sigma.w[i] = 0.5 / (stateSize + sigma.kappa); + } + + // Scale the sigma points + // 1. Xi' = X0 + alpha*(Xi-X0) + Matrix X0 = sigma.M.col(0); + for (unsigned int i = 1; i < sigmaPointCount; i++) { + sigma.M.col(i, X0 + sigma.alpha * (sigma.M.col(i) - X0)); + } + + // 2. W0' = W0/alpha^2 + (1/alpha^2-1) + // Wi' = Wi/alpha^2 + sigma.w[0] = sigma.w[0] / sigma.alpha / sigma.alpha + (1 / sigma.alpha / sigma.alpha - 1); + for (unsigned int i = 1; i < sigmaPointCount; i++) { + sigma.w[i] = sigma.w[i] / sigma.alpha / sigma.alpha; + } + }*/ + + std::vector UnscentedKalmanFilter::getStateEstimate() const { + std::vector state(model->getStateSize()); + for (unsigned int i = 0; i < model->getStateSize(); i++) { + state[i].uncertainty(UType::MeanCovar); + state[i].npoints(model->getStateSize()); + state[i][MEAN] = xEstimated[i]; + state[i][COVAR()] = static_cast>(P.row(i)); + } + return state; + } +} From ebea3eb080c8fbaaaf4e70e12f6ffa44ceb040e6 Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Tue, 21 Aug 2018 15:27:43 -0700 Subject: [PATCH 05/11] Draft of Model.h stateJacobianMatrix with Matrix mathematical ops, state_type helper math functions defined but unused --- inc/Model.h | 127 ++++++++++++++++++++++++- src/Observers/ExtendedKalmanFilter.cpp | 53 ++++------- 2 files changed, 144 insertions(+), 36 deletions(-) diff --git a/inc/Model.h b/inc/Model.h index 093c804..0ff5a21 100644 --- a/inc/Model.h +++ b/inc/Model.h @@ -29,7 +29,7 @@ namespace PCOE { using DynamicArray::operator[]; }; - + class StateVector final : public DynamicArray { public: using DynamicArray::DynamicArray; @@ -37,8 +37,11 @@ namespace PCOE { using DynamicArray::operator=; using DynamicArray::operator[]; + }; + + /** * Represents a State-space representation model of a system. In particular, * The following variables are used throughout the interface as defined @@ -70,6 +73,7 @@ namespace PCOE { using event_state_type = double; using noise_type = std::vector; + /** * Initializes the model with the given parameters. * @@ -141,6 +145,79 @@ namespace PCOE { const input_type& u, const noise_type& n) const = 0; + + /** + * Calculate the Jacobian for the state equation. Useful for EKF. + * + * @param t Time + * @param x The model state vector at the current time step. + * @param u The model input vector at the current time step. + * @param n The process noise vector. + * @param z The model output vector at the current time step. + * @return A stateSize x stateSize Jacobian Matrix. + **/ + virtual const Matrix stateJacobianMatrix(const double t, + const state_type& x, + const input_type& u, + const noise_type& n, + double dt = 1.0, + double epsilon = 0.1) { + Matrix jacobian(stateSize,stateSize); //Checked Matrix.h, should be correct type + for (int i=0; i inputs; std::vector outputs; + + }; } diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp index ccb3460..bca2042 100644 --- a/src/Observers/ExtendedKalmanFilter.cpp +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -90,7 +90,7 @@ namespace PCOE { } - // Fix this for EKF + // Tentatively OK for EKF void ExtendedKalmanFilter::initialize(const double t0, const Model::state_type& x0, const Model::input_type& u0) { @@ -104,10 +104,6 @@ namespace PCOE { // Initialize P P = Q; - // Compute initial Jacobians for current state estimate - //using definition of the derivative - //how do I figure out step size? Iterate? User input? Just pick something? - // Compute corresponding output estimate std::vector zeroNoiseZ(model->getOutputSize()); zEstimated = model->outputEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); @@ -133,38 +129,13 @@ namespace PCOE { // 1. Predict log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - predict"); - // Compute sigma points for current state estimate - computeSigmaPoints(xEstimated, Q, sigmaX); - - // Propagate sigma points through state equation - Matrix Xkk1(model->getStateSize(), sigmaPointCount); - std::vector zeroNoise(model->getStateSize()); - for (unsigned int i = 0; i < sigmaPointCount; i++) { - auto x = Model::state_type(static_cast>(sigmaX.M.col(i))); - x = model->stateEqn(timestamp, x, uPrev, zeroNoise, dt); - Xkk1.col(i, x.vec()); - } + // xkk1 = f(k1k1,uPrev) //calc next state from model assuming no noise + // ykk1 = h(xkk1) //calc next expected sensor readings from expected state + // F = jacobian(xkk1,ykk1) //get jacobian eval'd at new expected state, sensor output + // Update Pkk1 using F,Pk1k1,Q + // H = jacobian(xkk1, - // Recombine weighted sigma points to produce predicted state and covariance - std::vector xkk1 = - static_cast>(Xkk1.weightedMean(Matrix(sigmaX.w))); - Matrix Pkk1 = Xkk1.weightedCovariance(Matrix(sigmaX.w), sigmaX.alpha, sigmaX.beta) + Q; - - // Propagate sigma points through output equation - Matrix Zkk1(model->getOutputSize(), sigmaPointCount); - for (unsigned int i = 0; i < sigmaPointCount; i++) { - auto zkk1 = - model->outputEqn(timestamp, - Model::state_type(static_cast>(Xkk1.col(i))), - u, - zeroNoise); - Zkk1.col(i, zkk1.vec()); - } - // Recombine weighted sigma points to produce predicted measurement and covariance - std::vector zkk1 = - static_cast>(Zkk1.weightedMean(Matrix(sigmaX.w))); - Matrix Pzz = Zkk1.weightedCovariance(Matrix(sigmaX.w), sigmaX.alpha, sigmaX.beta) + R; // 2. Update log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - update"); @@ -212,6 +183,18 @@ namespace PCOE { uPrev = u; } + /* EMPTY JACOBIAN PLACEHOLDER */ + const Matrix ExtendedKalmanFilter::jacobian(const Model::state_type& mx, const Model::input_type& u) { + //return an empty matrix until I've figured out inputs/outputs + Matrix j + return j + } + + + + + + /*void ExtendedKalmanFilter::computeSigmaPoints(const Model::state_type& mx, const Matrix& Pxx, SigmaPoints& sigma) { From 6361215b129864206597a8ea8f6b20f4a855b528 Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Thu, 30 Aug 2018 15:34:31 -0700 Subject: [PATCH 06/11] Jacobian functions in Model.h, DynamicArray elementwise operators added, Matrix constructor from DynamicArray --- inc/DynamicArray.h | 34 ++++++++++- inc/Matrix.h | 10 +++- inc/Model.h | 143 ++++++++++++++++++++++----------------------- src/Matrix.cpp | 4 ++ 4 files changed, 115 insertions(+), 76 deletions(-) diff --git a/inc/DynamicArray.h b/inc/DynamicArray.h index b013dc4..db5ac1b 100644 --- a/inc/DynamicArray.h +++ b/inc/DynamicArray.h @@ -54,7 +54,7 @@ namespace PCOE { swap(*this, other); return *this; } - + friend void swap(DynamicArray& first, DynamicArray& second) { using std::swap; @@ -97,6 +97,38 @@ namespace PCOE { } return result; } + + /** + * Elementwise addition + */ + inline DynamicArray& operator+=(DynamicArray& toAdd) { + for (size_type i = 0; i < len; ++i) { + this->data[i] += toAdd[i]; + } + return *this; + } + + inline friend DynamicArray operator+(DynamicArray lhs, const DynamicArray& rhs){ + return lhs += rhs; + } + + + + /** + * Elementwise subtraction + */ + inline DynamicArray& operator-=(DynamicArray& toSubtract) { + for (size_type i = 0; i < len; ++i) { + this->data[i] -= toSubtract[i]; + } + return *this; + } + + inline friend DynamicArray operator-(DynamicArray lhs, const DynamicArray& rhs){ + return lhs -= rhs; + } + + private: T* data; diff --git a/inc/Matrix.h b/inc/Matrix.h index 9a23cb9..002e7c4 100644 --- a/inc/Matrix.h +++ b/inc/Matrix.h @@ -19,6 +19,7 @@ #include #include #include +#include "DynamicArray.h" namespace PCOE { #undef minor @@ -67,7 +68,14 @@ namespace PCOE { * @param v The values that will make up the vector */ explicit Matrix(const std::vector& v); - + + /** @brief Constructs a new M by 1 Matrix from a DynamicArray with + * the elements of @p . + * + * @param a The values that will make up the vector + */ + explicit Matrix(DynamicArray& a); + /** @brief Constructs a new Matrix by copying the elements of @p other. * * @param other A matrix of the same type and size from which to copy diff --git a/inc/Model.h b/inc/Model.h index 0ff5a21..422c1f5 100644 --- a/inc/Model.h +++ b/inc/Model.h @@ -37,6 +37,10 @@ namespace PCOE { using DynamicArray::operator=; using DynamicArray::operator[]; + + using DynamicArray::operator+=; + + using DynamicArray::operator-=; }; @@ -153,54 +157,42 @@ namespace PCOE { * @param x The model state vector at the current time step. * @param u The model input vector at the current time step. * @param n The process noise vector. - * @param z The model output vector at the current time step. - * @return A stateSize x stateSize Jacobian Matrix. + * @param dt The model output vector at the current time step. + * @return A stateSize by stateSize Jacobian Matrix. **/ - virtual const Matrix stateJacobianMatrix(const double t, - const state_type& x, - const input_type& u, - const noise_type& n, - double dt = 1.0, - double epsilon = 0.1) { - Matrix jacobian(stateSize,stateSize); //Checked Matrix.h, should be correct type - for (int i=0; i inputs; std::vector outputs; diff --git a/src/Matrix.cpp b/src/Matrix.cpp index 00f535a..484d670 100644 --- a/src/Matrix.cpp +++ b/src/Matrix.cpp @@ -48,6 +48,10 @@ namespace PCOE { Matrix::Matrix(const std::vector& v) : Matrix(v.size(), 1) { std::copy(v.cbegin(), v.cend(), data); } + + Matrix::Matrix(DynamicArray& a) : Matrix(a.size(), 1) { + std::copy(a.cbegin(), a.cend(), data); + } Matrix::Matrix(const Matrix& other) : M(other.M), N(other.N), data(new double[other.M * other.N]) { From a9499f497cbfb921a8ea9f22026e23d02e226d5a Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Thu, 30 Aug 2018 15:41:07 -0700 Subject: [PATCH 07/11] fixed comments for Jacobians in Model.h --- inc/Model.h | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/inc/Model.h b/inc/Model.h index 422c1f5..54e79e4 100644 --- a/inc/Model.h +++ b/inc/Model.h @@ -157,7 +157,8 @@ namespace PCOE { * @param x The model state vector at the current time step. * @param u The model input vector at the current time step. * @param n The process noise vector. - * @param dt The model output vector at the current time step. + * @param dt The size of the time step to calculate. + * @param epsilon the step size to use in the definition of the derivative. * @return A stateSize by stateSize Jacobian Matrix. **/ virtual Matrix getStateJacobian(const double t, @@ -174,11 +175,11 @@ namespace PCOE { state_type x_plus = x + diff; state_type x_minus = x - diff; - //partial derivatives of all state eq'ns w.r.t. xi + //state equations of perturbed xi's x_plus = stateEqn(t, x_plus, u, n, dt); x_minus = stateEqn(t, x_minus, u, n, dt); - //convert to Matrix form and differentiate + //convert to Matrix form and differentiate (w.r.t. xi) Matrix x_p(x_plus); Matrix x_m(x_minus); @@ -200,6 +201,7 @@ namespace PCOE { * @param u The model input vector at the current time step. * @param n The process noise vector. * @param z The model output vector at the current time step. + * @param epsilon the step size to use in the definition of the derivative. * @return An outputSize by stateSize Jacobian. **/ virtual Matrix getOutputJacobian(const double t, @@ -209,7 +211,7 @@ namespace PCOE { double epsilon = 0.01) const { Matrix jacobian(outputSize,stateSize); for (size_type i = 0; i < stateSize; i++) { - //perturb state vectors + //perturb state vectors by epsilon/2 state_type diff = getStateVector(0.0); diff[i] = epsilon/2.0; state_type x_plus = x + diff; @@ -219,7 +221,7 @@ namespace PCOE { x_plus = outputEqn(t, x_plus, u, n); x_minus = outputEqn(t, x_minus, u, n); - //convert to Matrix form and differentiate + //convert to Matrix form and differentiate (w.r.t. xi) Matrix x_p(x_plus); Matrix x_m(x_minus); From fa24f0b0dd8136500fd6bd2dd57f0eddf2fba147 Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Tue, 4 Sep 2018 18:34:05 -0700 Subject: [PATCH 08/11] Moved EKF to Observers folder and updated CMakeLists --- CMakeLists.txt | 2 +- inc/ExtendedKalmanFilter.h | 166 ------------------------- src/Observers/ExtendedKalmanFilter.cpp | 99 +++------------ 3 files changed, 21 insertions(+), 246 deletions(-) delete mode 100644 inc/ExtendedKalmanFilter.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ad325f4..fa4e02d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,7 +58,6 @@ set (HEADERS inc/DataPoints.h inc/DataStore.h inc/Datum.h - inc/ExtendedKalmanFilter.h inc/EventDrivenPrognoser.h inc/Exceptions.h inc/Factory.h @@ -79,6 +78,7 @@ set (HEADERS inc/Messages/UDataMessage.h inc/Model.h inc/Observers/EventDrivenObserver.h + inc/Observers/ExtendedKalmanFilter.h inc/Observers/Observer.h inc/Observers/ObserverFactory.h inc/Observers/ParticleFilter.h diff --git a/inc/ExtendedKalmanFilter.h b/inc/ExtendedKalmanFilter.h deleted file mode 100644 index 8e7f1b4..0000000 --- a/inc/ExtendedKalmanFilter.h +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright (c) 2018 United States Government as represented by the -// Administrator of the National Aeronautics and Space Administration. -// All Rights Reserved. - -#ifndef PCOE_EXTENDEDKALMANFILTER_H -#define PCOE_EXTENDEDKALMANFILTER_H - - -#endif - -/** - Work in progress: Still deciding how best to deal with Jacobians -**/ - -#include -#include - -#include "Matrix.h" -#include "Observers/Observer.h" - -namespace PCOE { - class ConfigMap; - - /** - * Implements EKF state estimation algorithm for non-linear models. Or at least will by the time I'm done with it. - * - ** - *** Denotes a note to myself that must be dealt with - ** - * - * @author Molly O'Connor - * Cannibalized from UnscentedKalmanFilter.h by - * @author Matthew Daigle - * @author Jason Watkins - **/ - class ExtendedKalmanFilter final : public Observer { - private: - /** - * Constructs a new @{code ExtendedKalmanFilter} instance and - * initializes the model. This constructor is only intended to be used - * by other constructors in UKF to set up model-related parameters. - * - * @param m A valid pointer to a model on which state estimation will be - * performed. The UKF does not take ownership of the model. - **/ - explicit ExtendedKalmanFilter(const Model* m); - - public: - /** - * Constructs a new @{code ExtendedKalmanFilter} instance with the - * given model and covariance matrices. - * - * @param m A valid pointer to a model on which state estimation will be - * performed. The EKF does not take ownership of the model. - * @param Q Process noise covariance matrix - * @param R Sensor noise covariance matrix - **/ - ExtendedKalmanFilter(const Model* m, const Matrix Q, const Matrix R); - - /** - * Constructs a new @{code UnscentedKalmanFilter} instance with the - * given model and with covariance matrices read from the provided - * config. - * - * @param m A valid pointer to a model on which state estimation - * will be performed. The UKF does not take ownership of - * the model. - * @param config A configuration from which to read covariance matrices. - **/ - ExtendedKalmanFilter(const Model* m, const ConfigMap& config); - - /** - * Sets the initial model state and computes initial sigma points. - * - * @param t0 Initial time - * @param x0 Initial model state - * @param u0 Initial model input - **/ - void initialize(double t0, - const Model::state_type& x0, - const Model::input_type& u0) override; - - /** - * Performs a single state estimation with the given model inputs and - * outputs. - * - * @param t The time at which to make a prediction. - * @param u The model input vector at time @{code t}. - * @param z The model output vector at time @{code t}. - **/ - void step(double t, const Model::input_type& u, const Model::output_type& z) override; - - /** - * Computes Jacobian for provided matrix using definition of a derivative in the case that no Jacobian is provided by user. - * Epsilon should be either a default value or provided by user. - * - * - ** - *** Correct input parameters? - ** - * - * @param mx Mean vector (should be last updated -- k|k-1). - * @param u From model, current u - * - **/ - const Matrix getJacobian(const Model::state_type& mx, const Model::input_type& u); - - /** - * - ** - *** - **** What does this do? - *** - ** - * - * Returns the current mean state estimate of the observer. - * - * @return The last calculated state estimate. - **/ - inline const Model::state_type& getStateMean() const override { - return xEstimated; - } - - /** - * - * Returns the current state estimate of the observer, including - * uncertainty. - * - * @return The last calculated state estimate calcualted by the - * observer. - **/ - std::vector getStateEstimate() const; - - /** - * - ** - *** - **** what - *** - ** - * - * Returns the current mean output estimate of the observer. - * - * @return The last output estimate calcualted by the observer.. - **/ - inline const Model::output_type& getOutputMean() const override { - return zEstimated; - } - - /** - * Gets the state covariance matrix. - **/ - const Matrix& getStateCovariance() const { - return P; - } - - private: - Model::state_type xEstimated; - Model::output_type zEstimated; - Matrix Q; - Matrix R; - Matrix P; - }; -} - -#endif diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp index bca2042..6bda9f7 100644 --- a/src/Observers/ExtendedKalmanFilter.cpp +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -4,7 +4,7 @@ #include -#include "ExtendedKalmanFilter.h" +//#include "ExtendedKalmanFilter.h" /** @@ -18,7 +18,7 @@ #include "Exceptions.h" #include "ConfigMap.h" -//#include "Observers/ExtendedKalmanFilter.h" +#include "Observers/ExtendedKalmanFilter.h" #include "ThreadSafeLog.h" #include "UData.h" @@ -120,6 +120,9 @@ namespace PCOE { Expect(isInitialized(), "Not initialized"); Expect(timestamp - lastTime > 0, "Time has not advanced"); + std::vector zeroNoiseZ(model->getOutputSize()); + + // Update time double dt = timestamp - lastTime; lastTime = timestamp; @@ -129,11 +132,22 @@ namespace PCOE { // 1. Predict log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - predict"); - // xkk1 = f(k1k1,uPrev) //calc next state from model assuming no noise - // ykk1 = h(xkk1) //calc next expected sensor readings from expected state + // 1. xkk1 = f(k1k1,uPrev) //calc next state from model assuming no noise + Model::state_type xkk1 = model->stateEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); + + // 2. ykk1 = h(xkk1) //calc next expected sensor readings from expected state + Model::output_type zkk1 = model->outputEqn(lastTime, xkk1, uPrev, zeroNoiseZ); + // F = jacobian(xkk1,ykk1) //get jacobian eval'd at new expected state, sensor output - // Update Pkk1 using F,Pk1k1,Q - // H = jacobian(xkk1, + Matrix stateJacobian = model->getStateJacobian(lastTime, xkk1, uPrev, zeroNoiseZ); + + // 3. Update Pkk1 using F,Pk1k1,Q + // H = jacobian(xkk1,u) + // 4. Update Pyy using H, Pkk1, R=sensor noise covariance + // 5. Update Pxy + // 6. Update Kalman Gain Kk + // 7. Update xkk + // 8. Update Pkk @@ -183,79 +197,6 @@ namespace PCOE { uPrev = u; } - /* EMPTY JACOBIAN PLACEHOLDER */ - const Matrix ExtendedKalmanFilter::jacobian(const Model::state_type& mx, const Model::input_type& u) { - //return an empty matrix until I've figured out inputs/outputs - Matrix j - return j - } - - - - - - - /*void ExtendedKalmanFilter::computeSigmaPoints(const Model::state_type& mx, - const Matrix& Pxx, - SigmaPoints& sigma) { - log.WriteLine(LOG_TRACE, MODULE_NAME, "Computing sigma points"); - - // Assumes that sigma points have been set up correctly within the constructor - auto stateSize = mx.size(); - auto sigmaPointCount = sigma.M.cols(); - - // First sigma point is the mean - for (unsigned int i = 0; i < stateSize; i++) { - sigma.M[i][0] = mx[i]; - } - - // Compute a matrix square root using Cholesky decomposition - Matrix nkPxx = Pxx; - for (unsigned int i = 0; i < nkPxx.rows(); i++) { - for (unsigned int j = 0; j < nkPxx.cols(); j++) { - nkPxx[i][j] *= (stateSize + sigma.kappa); - } - } - Matrix matrixSq = nkPxx.chol(); - - // For sigma points 2 to n+1, it is mx + ith column of matrix square root - for (unsigned int i = 0; i < stateSize; i++) { - // Set column - for (unsigned int j = 0; j < stateSize; j++) { - sigma.M[i][j + 1] = mx[i] + matrixSq[i][j]; - } - } - - // For sigma points n+2 to 2n+1, it is mx - ith column of matrix square root - for (unsigned int i = 0; i < stateSize; i++) { - // Set column - for (unsigned int j = 0; j < stateSize; j++) { - sigma.M[i][j + stateSize + 1] = mx[i] - matrixSq[i][j]; - } - } - - // w(1) is kappa/(n+kappa) - sigma.w[0] = sigma.kappa / (stateSize + sigma.kappa); - - // Rest of w are 0.5/(n+kappa) - for (unsigned int i = 1; i < sigmaPointCount; i++) { - sigma.w[i] = 0.5 / (stateSize + sigma.kappa); - } - - // Scale the sigma points - // 1. Xi' = X0 + alpha*(Xi-X0) - Matrix X0 = sigma.M.col(0); - for (unsigned int i = 1; i < sigmaPointCount; i++) { - sigma.M.col(i, X0 + sigma.alpha * (sigma.M.col(i) - X0)); - } - - // 2. W0' = W0/alpha^2 + (1/alpha^2-1) - // Wi' = Wi/alpha^2 - sigma.w[0] = sigma.w[0] / sigma.alpha / sigma.alpha + (1 / sigma.alpha / sigma.alpha - 1); - for (unsigned int i = 1; i < sigmaPointCount; i++) { - sigma.w[i] = sigma.w[i] / sigma.alpha / sigma.alpha; - } - }*/ std::vector UnscentedKalmanFilter::getStateEstimate() const { std::vector state(model->getStateSize()); From 6fc9fcdff3c0c59fd51963af7b118c7f31b8b35f Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Tue, 4 Sep 2018 18:47:18 -0700 Subject: [PATCH 09/11] Moved EKF to Observers folder and updated CMakeLists --- inc/Observers/ExtendedKalmanFilter.h | 148 +++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 inc/Observers/ExtendedKalmanFilter.h diff --git a/inc/Observers/ExtendedKalmanFilter.h b/inc/Observers/ExtendedKalmanFilter.h new file mode 100644 index 0000000..3318fab --- /dev/null +++ b/inc/Observers/ExtendedKalmanFilter.h @@ -0,0 +1,148 @@ +// Copyright (c) 2018 United States Government as represented by the +// Administrator of the National Aeronautics and Space Administration. +// All Rights Reserved. + +#ifndef PCOE_EXTENDEDKALMANFILTER_H +#define PCOE_EXTENDEDKALMANFILTER_H + + +#endif + + +#include +#include + +#include "Matrix.h" +#include "Observers/Observer.h" + +namespace PCOE { + class ConfigMap; + + /** + * Implements EKF state estimation algorithm for non-linear models. Or at least will by the time I'm done with it. + * + ** + *** Denotes a note to myself that must be dealt with + ** + * + * @author Molly O'Connor + * Cannibalized from UnscentedKalmanFilter.h by + * @author Matthew Daigle + * @author Jason Watkins + **/ + class ExtendedKalmanFilter final : public Observer { + private: + /** + * Constructs a new @{code ExtendedKalmanFilter} instance and + * initializes the model. This constructor is only intended to be used + * by other constructors in UKF to set up model-related parameters. + * + * @param m A valid pointer to a model on which state estimation will be + * performed. The UKF does not take ownership of the model. + **/ + explicit ExtendedKalmanFilter(const Model* m); + + public: + /** + * Constructs a new @{code ExtendedKalmanFilter} instance with the + * given model and covariance matrices. + * + * @param m A valid pointer to a model on which state estimation will be + * performed. The EKF does not take ownership of the model. + * @param Q Process noise covariance matrix + * @param R Sensor noise covariance matrix + **/ + ExtendedKalmanFilter(const Model* m, const Matrix Q, const Matrix R); + + /** + * Constructs a new @{code UnscentedKalmanFilter} instance with the + * given model and with covariance matrices read from the provided + * config. + * + * @param m A valid pointer to a model on which state estimation + * will be performed. The UKF does not take ownership of + * the model. + * @param config A configuration from which to read covariance matrices. + **/ + ExtendedKalmanFilter(const Model* m, const ConfigMap& config); + + /** + * Sets the initial model state and computes initial sigma points. + * + * @param t0 Initial time + * @param x0 Initial model state + * @param u0 Initial model input + **/ + void initialize(double t0, + const Model::state_type& x0, + const Model::input_type& u0) override; + + /** + * Performs a single state estimation with the given model inputs and + * outputs. + * + * @param t The time at which to make a prediction. + * @param u The model input vector at time @{code t}. + * @param z The model output vector at time @{code t}. + **/ + void step(double t, const Model::input_type& u, const Model::output_type& z) override; + + /** + * + ** + *** + **** What does this do? + *** + ** + * + * Returns the current mean state estimate of the observer. + * + * @return The last calculated state estimate. + **/ + inline const Model::state_type& getStateMean() const override { + return xEstimated; + } + + /** + * + * Returns the current state estimate of the observer, including + * uncertainty. + * + * @return The last calculated state estimate calcualted by the + * observer. + **/ + std::vector getStateEstimate() const; + + /** + * + ** + *** + **** what + *** + ** + * + * Returns the current mean output estimate of the observer. + * + * @return The last output estimate calcualted by the observer.. + **/ + inline const Model::output_type& getOutputMean() const override { + return zEstimated; + } + + /** + * Gets the state covariance matrix. + **/ + const Matrix& getStateCovariance() const { + return P; + } + + private: + Model::state_type xEstimated; + Model::output_type zEstimated; + Matrix Q; + Matrix R; + Matrix P; + }; +} + +#endif From 629605431ca3f29c9baf2037a6a5c87930481419 Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Wed, 5 Sep 2018 10:48:59 -0700 Subject: [PATCH 10/11] Fixed model pointers in EKF --- inc/Observers/ExtendedKalmanFilter.h | 40 ++---------------- src/Observers/ExtendedKalmanFilter.cpp | 58 +++++++++++++------------- 2 files changed, 32 insertions(+), 66 deletions(-) diff --git a/inc/Observers/ExtendedKalmanFilter.h b/inc/Observers/ExtendedKalmanFilter.h index 3318fab..8925652 100644 --- a/inc/Observers/ExtendedKalmanFilter.h +++ b/inc/Observers/ExtendedKalmanFilter.h @@ -40,7 +40,7 @@ namespace PCOE { * @param m A valid pointer to a model on which state estimation will be * performed. The UKF does not take ownership of the model. **/ - explicit ExtendedKalmanFilter(const Model* m); + explicit ExtendedKalmanFilter(const Model& m); public: /** @@ -52,7 +52,7 @@ namespace PCOE { * @param Q Process noise covariance matrix * @param R Sensor noise covariance matrix **/ - ExtendedKalmanFilter(const Model* m, const Matrix Q, const Matrix R); + ExtendedKalmanFilter(const Model& m, const Matrix Q, const Matrix R); /** * Constructs a new @{code UnscentedKalmanFilter} instance with the @@ -64,7 +64,7 @@ namespace PCOE { * the model. * @param config A configuration from which to read covariance matrices. **/ - ExtendedKalmanFilter(const Model* m, const ConfigMap& config); + ExtendedKalmanFilter(const Model& m, const ConfigMap& config); /** * Sets the initial model state and computes initial sigma points. @@ -87,22 +87,6 @@ namespace PCOE { **/ void step(double t, const Model::input_type& u, const Model::output_type& z) override; - /** - * - ** - *** - **** What does this do? - *** - ** - * - * Returns the current mean state estimate of the observer. - * - * @return The last calculated state estimate. - **/ - inline const Model::state_type& getStateMean() const override { - return xEstimated; - } - /** * * Returns the current state estimate of the observer, including @@ -111,23 +95,7 @@ namespace PCOE { * @return The last calculated state estimate calcualted by the * observer. **/ - std::vector getStateEstimate() const; - - /** - * - ** - *** - **** what - *** - ** - * - * Returns the current mean output estimate of the observer. - * - * @return The last output estimate calcualted by the observer.. - **/ - inline const Model::output_type& getOutputMean() const override { - return zEstimated; - } + std::vector getStateEstimate() const override; /** * Gets the state covariance matrix. diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp index 6bda9f7..81d9152 100644 --- a/src/Observers/ExtendedKalmanFilter.cpp +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -33,26 +33,25 @@ namespace PCOE { const std::string MODULE_NAME = "OBS-EKF"; - ExtendedKalmanFilter::ExtendedKalmanFilter(const Model* m) : Observer(m) { - Expect(m != nullptr, "Invalid model"); - xEstimated = model->getStateVector(); - uPrev = model->getInputVector(); - zEstimated = model->getOutputVector(); + ExtendedKalmanFilter::ExtendedKalmanFilter(const Model& m) : Observer(m) { + xEstimated = model.getStateVector(); + uPrev = model.getInputVector(); + zEstimated = model.getOutputVector(); } - ExtendedKalmanFilter::ExtendedKalmanFilter(const Model* m, Matrix q, Matrix r) + ExtendedKalmanFilter::ExtendedKalmanFilter(const Model& m, Matrix q, Matrix r) : ExtendedKalmanFilter(m) { Expect(q.rows() == q.cols(), "q is not square"); - Expect(q.rows() == model->getStateSize(), "Size of q does not match model state size"); + Expect(q.rows() == model.getStateSize(), "Size of q does not match model state size"); Expect(r.rows() == r.cols(), "q is not square"); - Expect(r.rows() == model->getOutputSize(), "Size of r does not match model output size"); + Expect(r.rows() == model.getOutputSize(), "Size of r does not match model output size"); Q = std::move(q); R = std::move(r); } // ConfigMap-based Constructor - ExtendedKalmanFilter::ExtendedKalmanFilter(const Model* model, const ConfigMap& config) + ExtendedKalmanFilter::ExtendedKalmanFilter(const Model& model, const ConfigMap& config) : ExtendedKalmanFilter(model) { requireKeys(config, {Q_KEY, R_KEY}); @@ -105,8 +104,8 @@ namespace PCOE { P = Q; // Compute corresponding output estimate - std::vector zeroNoiseZ(model->getOutputSize()); - zEstimated = model->outputEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); + std::vector zeroNoiseZ(model.getOutputSize()); + zEstimated = model.outputEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); // Set initialized flag initialized = true; @@ -120,26 +119,25 @@ namespace PCOE { Expect(isInitialized(), "Not initialized"); Expect(timestamp - lastTime > 0, "Time has not advanced"); - std::vector zeroNoiseZ(model->getOutputSize()); + std::vector zeroNoiseZ(model.getOutputSize()); // Update time double dt = timestamp - lastTime; lastTime = timestamp; - std::size_t sigmaPointCount = sigmaX.M.cols(); // 1. Predict log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - predict"); // 1. xkk1 = f(k1k1,uPrev) //calc next state from model assuming no noise - Model::state_type xkk1 = model->stateEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); + Model::state_type xkk1 = model.stateEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); // 2. ykk1 = h(xkk1) //calc next expected sensor readings from expected state - Model::output_type zkk1 = model->outputEqn(lastTime, xkk1, uPrev, zeroNoiseZ); + Model::output_type zkk1 = model.outputEqn(lastTime, xkk1, uPrev, zeroNoiseZ); // F = jacobian(xkk1,ykk1) //get jacobian eval'd at new expected state, sensor output - Matrix stateJacobian = model->getStateJacobian(lastTime, xkk1, uPrev, zeroNoiseZ); + Matrix stateJacobian = model.getStateJacobian(lastTime, xkk1, uPrev, zeroNoiseZ); // 3. Update Pkk1 using F,Pk1k1,Q // H = jacobian(xkk1,u) @@ -155,15 +153,15 @@ namespace PCOE { log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - update"); // Compute state-output cross-covariance matrix - Matrix Pxz(model->getStateSize(), model->getOutputSize()); + Matrix Pxz(model.getStateSize(), model.getOutputSize()); for (unsigned int i = 0; i < sigmaPointCount; i++) { - Matrix columnx(model->getStateSize(), 1); - Matrix columnz(model->getOutputSize(), 1); + Matrix columnx(model.getStateSize(), 1); + Matrix columnz(model.getOutputSize(), 1); columnx.col(0, Xkk1.col(i)); columnz.col(0, Zkk1.col(i)); - Matrix xkk1m(model->getStateSize(), 1); - Matrix zkk1m(model->getOutputSize(), 1); + Matrix xkk1m(model.getStateSize(), 1); + Matrix zkk1m(model.getOutputSize(), 1); xkk1m.col(0, xkk1); zkk1m.col(0, zkk1); @@ -177,9 +175,9 @@ namespace PCOE { Matrix Kk = Pxz * Pzz.inverse(); // Compute state estimate - Matrix xkk1m(model->getStateSize(), 1); - Matrix zkk1m(model->getOutputSize(), 1); - Matrix zm(model->getOutputSize(), 1); + Matrix xkk1m(model.getStateSize(), 1); + Matrix zkk1m(model.getOutputSize(), 1); + Matrix zm(model.getOutputSize(), 1); xkk1m.col(0, xkk1); zkk1m.col(0, zkk1); zm.col(0, z.vec()); @@ -187,8 +185,8 @@ namespace PCOE { xEstimated = Model::state_type(static_cast>(xk1m.col(0))); // Compute output estimate - std::vector zeroNoiseZ(model->getOutputSize()); - zEstimated = model->outputEqn(timestamp, xEstimated, u, zeroNoiseZ); + std::vector zeroNoiseZ(model.getOutputSize()); + zEstimated = model.outputEqn(timestamp, xEstimated, u, zeroNoiseZ); // Compute covariance P = Pkk1 - Kk * Pzz * Kk.transpose(); @@ -198,11 +196,11 @@ namespace PCOE { } - std::vector UnscentedKalmanFilter::getStateEstimate() const { - std::vector state(model->getStateSize()); - for (unsigned int i = 0; i < model->getStateSize(); i++) { + std::vector ExtendedKalmanFilter::getStateEstimate() const { + std::vector state(model.getStateSize()); + for (unsigned int i = 0; i < model.getStateSize(); i++) { state[i].uncertainty(UType::MeanCovar); - state[i].npoints(model->getStateSize()); + state[i].npoints(model.getStateSize()); state[i][MEAN] = xEstimated[i]; state[i][COVAR()] = static_cast>(P.row(i)); } From 953a0d0e6d5abec518ad05544db6a1ba00da9b25 Mon Sep 17 00:00:00 2001 From: Molly O'Connor Date: Wed, 5 Sep 2018 17:16:38 -0700 Subject: [PATCH 11/11] prelim. EKF implemented, timesteps might be off and need examining --- inc/Model.h | 1 - inc/Observers/ExtendedKalmanFilter.h | 20 +++--- src/Observers/ExtendedKalmanFilter.cpp | 92 ++++++++++---------------- 3 files changed, 45 insertions(+), 68 deletions(-) diff --git a/inc/Model.h b/inc/Model.h index 54e79e4..b247147 100644 --- a/inc/Model.h +++ b/inc/Model.h @@ -141,7 +141,6 @@ namespace PCOE { * @param x The model state vector at the current time step. * @param u The model input vector at the current time step. * @param n The process noise vector. - * @param z The model output vector at the current time step. * @return The model output vector at the next time step. **/ virtual output_type outputEqn(const double t, diff --git a/inc/Observers/ExtendedKalmanFilter.h b/inc/Observers/ExtendedKalmanFilter.h index 8925652..0c16dc6 100644 --- a/inc/Observers/ExtendedKalmanFilter.h +++ b/inc/Observers/ExtendedKalmanFilter.h @@ -19,7 +19,7 @@ namespace PCOE { class ConfigMap; /** - * Implements EKF state estimation algorithm for non-linear models. Or at least will by the time I'm done with it. + * Implements EKF state estimation algorithm for non-linear models. * ** *** Denotes a note to myself that must be dealt with @@ -35,17 +35,19 @@ namespace PCOE { /** * Constructs a new @{code ExtendedKalmanFilter} instance and * initializes the model. This constructor is only intended to be used - * by other constructors in UKF to set up model-related parameters. + * by other constructors in EKF to set up model-related parameters. * * @param m A valid pointer to a model on which state estimation will be - * performed. The UKF does not take ownership of the model. + * performed. The EKF does not take ownership of the model. **/ explicit ExtendedKalmanFilter(const Model& m); public: /** * Constructs a new @{code ExtendedKalmanFilter} instance with the - * given model and covariance matrices. + * given model and covariance matrices. Checks that noise covariance + * matrices are square matrices with the same dimensions as model state + * and output vectors. * * @param m A valid pointer to a model on which state estimation will be * performed. The EKF does not take ownership of the model. @@ -55,19 +57,19 @@ namespace PCOE { ExtendedKalmanFilter(const Model& m, const Matrix Q, const Matrix R); /** - * Constructs a new @{code UnscentedKalmanFilter} instance with the + * Constructs a new @{code ExtendedKalmanFilter} instance with the * given model and with covariance matrices read from the provided * config. * * @param m A valid pointer to a model on which state estimation - * will be performed. The UKF does not take ownership of + * will be performed. The EKF does not take ownership of * the model. * @param config A configuration from which to read covariance matrices. **/ ExtendedKalmanFilter(const Model& m, const ConfigMap& config); /** - * Sets the initial model state and computes initial sigma points. + * Sets the initial model state. * * @param t0 Initial time * @param x0 Initial model state @@ -81,11 +83,11 @@ namespace PCOE { * Performs a single state estimation with the given model inputs and * outputs. * - * @param t The time at which to make a prediction. + * @param t The time at which to make a prediction. Must have advanced from previous time. * @param u The model input vector at time @{code t}. * @param z The model output vector at time @{code t}. **/ - void step(double t, const Model::input_type& u, const Model::output_type& z) override; + void step(double t, const Model::input_type& u, Model::output_type& z) override; /** * diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp index 81d9152..6d5a5cc 100644 --- a/src/Observers/ExtendedKalmanFilter.cpp +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -89,7 +89,7 @@ namespace PCOE { } - // Tentatively OK for EKF + // Initialize the EKF void ExtendedKalmanFilter::initialize(const double t0, const Model::state_type& x0, const Model::input_type& u0) { @@ -100,12 +100,12 @@ namespace PCOE { xEstimated = x0; uPrev = u0; - // Initialize P + // Initialize P (state covariance matrix) as Q (process noise covariance matrix) P = Q; // Compute corresponding output estimate - std::vector zeroNoiseZ(model.getOutputSize()); - zEstimated = model.outputEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); + std::vector zeroOutputNoise(model.getOutputSize()); + zEstimated = model.outputEqn(lastTime, xEstimated, uPrev, zeroOutputNoise); // Set initialized flag initialized = true; @@ -114,85 +114,61 @@ namespace PCOE { void ExtendedKalmanFilter::step(const double timestamp, const Model::input_type& u, - const Model::output_type& z) { + Model::output_type& z) { log.WriteLine(LOG_DEBUG, MODULE_NAME, "Starting step"); Expect(isInitialized(), "Not initialized"); Expect(timestamp - lastTime > 0, "Time has not advanced"); - std::vector zeroNoiseZ(model.getOutputSize()); - + std::vector zeroProcessNoise(model.getStateSize()); + std::vector zeroOutputNoise(model.getOutputSize()); // Update time double dt = timestamp - lastTime; lastTime = timestamp; - // 1. Predict + //*/*/* Prediction Step */*/*// log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - predict"); - // 1. xkk1 = f(k1k1,uPrev) //calc next state from model assuming no noise - Model::state_type xkk1 = model.stateEqn(lastTime, xEstimated, uPrev, zeroNoiseZ); - - // 2. ykk1 = h(xkk1) //calc next expected sensor readings from expected state - Model::output_type zkk1 = model.outputEqn(lastTime, xkk1, uPrev, zeroNoiseZ); + // 1. x_k|k-1 = f(k1k1,uPrev) //calc next state from model assuming no noise + Model::state_type xkk1 = model.stateEqn(timestamp, xEstimated, uPrev, zeroProcessNoise); - // F = jacobian(xkk1,ykk1) //get jacobian eval'd at new expected state, sensor output - Matrix stateJacobian = model.getStateJacobian(lastTime, xkk1, uPrev, zeroNoiseZ); + // 2. z_k|k-1 = h(xkk1) //calc next expected sensor readings from expected state + Model::output_type zkk1 = model.outputEqn(timestamp, xkk1, uPrev, zeroOutputNoise); - // 3. Update Pkk1 using F,Pk1k1,Q - // H = jacobian(xkk1,u) - // 4. Update Pyy using H, Pkk1, R=sensor noise covariance - // 5. Update Pxy - // 6. Update Kalman Gain Kk - // 7. Update xkk - // 8. Update Pkk + //*/*/* Update Step */*/*// + log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - update"); + // F = stateJacobian(xkk1,uPrev) //get jacobian eval'd at new expected state, sensor output + Matrix stateJacobian = model.getStateJacobian(timestamp, xkk1, u, zeroProcessNoise, dt); + // H = outputJacobian(xkk1,u) + Matrix outputJacobian = model.getOutputJacobian(timestamp, xkk1, u, zeroOutputNoise); + // 3. Update P_k|k-1 using F,P_k-1|k-1,Q + Matrix Pkk1 = stateJacobian * P * stateJacobian.transpose() + Q; - // 2. Update - log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - update"); + // 4. Update Pzz using H, Pkk1, R=sensor noise covariance + Matrix Pzz = outputJacobian * Pkk1 * outputJacobian.transpose() + R; - // Compute state-output cross-covariance matrix - Matrix Pxz(model.getStateSize(), model.getOutputSize()); - for (unsigned int i = 0; i < sigmaPointCount; i++) { - Matrix columnx(model.getStateSize(), 1); - Matrix columnz(model.getOutputSize(), 1); - columnx.col(0, Xkk1.col(i)); - columnz.col(0, Zkk1.col(i)); - - Matrix xkk1m(model.getStateSize(), 1); - Matrix zkk1m(model.getOutputSize(), 1); - xkk1m.col(0, xkk1); - zkk1m.col(0, zkk1); - - Matrix diffx = columnx - xkk1m; - Matrix diffzT = (columnz - zkk1m).transpose(); - Matrix temp = diffx * diffzT; - Pxz = Pxz + temp * sigmaX.w[i]; - } + // 5. Update Pxz + Matrix Pxz = Pkk1 * outputJacobian.transpose(); - // Compute Kalman gain + // 6. Compute Kalman Gain Kk Matrix Kk = Pxz * Pzz.inverse(); - // Compute state estimate - Matrix xkk1m(model.getStateSize(), 1); - Matrix zkk1m(model.getOutputSize(), 1); - Matrix zm(model.getOutputSize(), 1); - xkk1m.col(0, xkk1); - zkk1m.col(0, zkk1); - zm.col(0, z.vec()); - Matrix xk1m = xkk1m + Kk * (zm - zkk1m); - xEstimated = Model::state_type(static_cast>(xk1m.col(0))); + // 7. Update state estimate + Matrix xkk = Matrix(xkk1) + (Kk * (Matrix(z) - Matrix(zkk1))); + xEstimated = Model::state_type(static_cast>(xkk.col(0))); - // Compute output estimate - std::vector zeroNoiseZ(model.getOutputSize()); - zEstimated = model.outputEqn(timestamp, xEstimated, u, zeroNoiseZ); + // 8. Update Pkk = (I-Kk*Hk)Pkk1 + P = (Matrix::identity(model.getStateSize()) - (Kk * outputJacobian)) * Pkk1; - // Compute covariance - P = Pkk1 - Kk * Pzz * Kk.transpose(); + // Compute output estimate + zEstimated = model.outputEqn(timestamp, xEstimated, u, zeroOutputNoise); - // Update uOld + //Update uPrev uPrev = u; + }