diff --git a/CMakeLists.txt b/CMakeLists.txt index b79c5dc..a51a484 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ set (HEADERS inc/Messages/WaypointMessage.h inc/Model.h inc/Observers/EventDrivenObserver.h + inc/Observers/ExtendedKalmanFilter.h inc/Observers/Observer.h inc/Observers/ObserverFactory.h inc/Observers/ParticleFilter.h @@ -136,6 +137,7 @@ set(SRCS src/Messages/MessageId.cpp src/Messages/WaypointMessage.cpp src/Observers/EventDrivenObserver.cpp + src/Observers/ExtendedKalmanFilter.cpp src/Observers/ParticleFilter.cpp src/Observers/UnscentedKalmanFilter.cpp src/PContainer.cpp diff --git a/inc/DynamicArray.h b/inc/DynamicArray.h index 0270bce..a0bdfbe 100644 --- a/inc/DynamicArray.h +++ b/inc/DynamicArray.h @@ -205,7 +205,7 @@ namespace PCOE { const_reference operator[](size_type pos) const { return storage[pos]; } - + /** * Returns a reference to the first element of the array. The behavior * of this method is undefined when the array has size 0. @@ -308,6 +308,38 @@ namespace PCOE { iterator rend() { return storage.end(); } + + /** + * 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; + } + + /** * Returns an constant iterator to the first element of the array. diff --git a/inc/Matrix.h b/inc/Matrix.h index 001272f..bd91867 100644 --- a/inc/Matrix.h +++ b/inc/Matrix.h @@ -10,6 +10,7 @@ #include #include #include +#include "DynamicArray.h" namespace PCOE { #undef minor @@ -66,7 +67,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 093c804..b247147 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,15 @@ namespace PCOE { using DynamicArray::operator=; using DynamicArray::operator[]; + + 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 +77,7 @@ namespace PCOE { using event_state_type = double; using noise_type = std::vector; + /** * Initializes the model with the given parameters. * @@ -133,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, @@ -141,6 +148,92 @@ 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 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, + const state_type& x, + const input_type& u, + const noise_type& n, + double dt = 1.0, + double epsilon = 0.01) const { + Matrix jacobian(stateSize,stateSize); + for (size_type i = 0; i < stateSize; i++) { + //perturb state vectors by epsilon/2 + state_type diff = getStateVector(0.0); + diff[i] = epsilon/2.0; + state_type x_plus = x + diff; + state_type x_minus = x - diff; + + //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 (w.r.t. xi) + Matrix x_p(x_plus); + Matrix x_m(x_minus); + + Matrix dx_i = x_p - x_m; + dx_i /= (epsilon); + + //set result as ith column of Jacobian + jacobian.col(i, dx_i); + } + return jacobian; + + } + + /** + * Calculate the Jacobian for the output 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. + * @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, + const state_type& x, + const input_type& u, + const noise_type& n, + double epsilon = 0.01) const { + Matrix jacobian(outputSize,stateSize); + for (size_type i = 0; i < stateSize; i++) { + //perturb state vectors by epsilon/2 + state_type diff = getStateVector(0.0); + diff[i] = epsilon/2.0; + state_type x_plus = x + diff; + state_type x_minus = x - diff; + + //output equations of perturbed xi's + x_plus = outputEqn(t, x_plus, u, n); + x_minus = outputEqn(t, x_minus, u, n); + + //convert to Matrix form and differentiate (w.r.t. xi) + Matrix x_p(x_plus); + Matrix x_m(x_minus); + + Matrix dx_i = x_p - x_m; + dx_i /= (epsilon); + + //set result as ith column of Jacobian + jacobian.col(i, dx_i); + } + return jacobian; + } + + /** * Initialize the model state. * @@ -156,6 +249,17 @@ namespace PCOE { inline state_type getStateVector() const { return state_type(stateSize); } + + /** + * Gets a state vector of the correct size for the current model, initialized to @p value. + **/ + inline state_type getStateVector(double value) const { + state_type st(stateSize); + for (size_type i = 0; i < stateSize; ++i) { + st[i] = value; + } + return st; + } /** * Gets the size of the the state vector. @@ -222,11 +326,28 @@ namespace PCOE { return outputs; } + /** + * Converts a state_type to a (stateSize x 1) Matrix + * New constructor for Matrix in Matrix.h that takes a DynamicArray + **/ + inline Matrix stateToMatrix(state_type& x){ + Matrix m(stateSize,1); + for (size_type i=0; i < stateSize; i++){ + m[i][0] = double(x[i]); + } + return m; + } + + private: double defaultTimeStep = 1.0; + //double defaultEpsilon = 0.01; state_type::size_type stateSize; + output_type::size_type outputSize; std::vector inputs; std::vector outputs; + + }; } diff --git a/inc/Observers/ExtendedKalmanFilter.h b/inc/Observers/ExtendedKalmanFilter.h new file mode 100644 index 0000000..0c16dc6 --- /dev/null +++ b/inc/Observers/ExtendedKalmanFilter.h @@ -0,0 +1,118 @@ +// 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. + * + ** + *** 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 EKF to set up model-related parameters. + * + * @param m A valid pointer to a model on which state estimation will be + * 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. 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. + * @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 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 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. + * + * @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. 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, Model::output_type& z) override; + + /** + * + * Returns the current state estimate of the observer, including + * uncertainty. + * + * @return The last calculated state estimate calcualted by the + * observer. + **/ + std::vector getStateEstimate() const override; + + /** + * 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/inc/Observers/Observer.h b/inc/Observers/Observer.h index 7e3fa86..d47a16f 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 **/ diff --git a/src/Matrix.cpp b/src/Matrix.cpp index 7e5e8b4..c5c7967 100644 --- a/src/Matrix.cpp +++ b/src/Matrix.cpp @@ -35,6 +35,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]) { diff --git a/src/Observers/ExtendedKalmanFilter.cpp b/src/Observers/ExtendedKalmanFilter.cpp new file mode 100644 index 0000000..6d5a5cc --- /dev/null +++ b/src/Observers/ExtendedKalmanFilter.cpp @@ -0,0 +1,185 @@ +// 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" + + +/** + 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) { + 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"); + } + + + // Initialize the 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 (state covariance matrix) as Q (process noise covariance matrix) + P = Q; + + // Compute corresponding output estimate + std::vector zeroOutputNoise(model.getOutputSize()); + zEstimated = model.outputEqn(lastTime, xEstimated, uPrev, zeroOutputNoise); + + // Set initialized flag + initialized = true; + log.WriteLine(LOG_DEBUG, MODULE_NAME, "Initialize completed"); + } + + void ExtendedKalmanFilter::step(const double timestamp, + const Model::input_type& u, + 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 zeroProcessNoise(model.getStateSize()); + std::vector zeroOutputNoise(model.getOutputSize()); + + // Update time + double dt = timestamp - lastTime; + lastTime = timestamp; + + + //*/*/* Prediction Step */*/*// + log.WriteLine(LOG_TRACE, MODULE_NAME, "Starting step - predict"); + + // 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); + + // 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); + + //*/*/* 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; + + // 4. Update Pzz using H, Pkk1, R=sensor noise covariance + Matrix Pzz = outputJacobian * Pkk1 * outputJacobian.transpose() + R; + + // 5. Update Pxz + Matrix Pxz = Pkk1 * outputJacobian.transpose(); + + // 6. Compute Kalman Gain Kk + Matrix Kk = Pxz * Pzz.inverse(); + + // 7. Update state estimate + Matrix xkk = Matrix(xkk1) + (Kk * (Matrix(z) - Matrix(zkk1))); + xEstimated = Model::state_type(static_cast>(xkk.col(0))); + + // 8. Update Pkk = (I-Kk*Hk)Pkk1 + P = (Matrix::identity(model.getStateSize()) - (Kk * outputJacobian)) * Pkk1; + + // Compute output estimate + zEstimated = model.outputEqn(timestamp, xEstimated, u, zeroOutputNoise); + + //Update uPrev + uPrev = u; + + } + + + 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][MEAN] = xEstimated[i]; + state[i][COVAR()] = static_cast>(P.row(i)); + } + return state; + } +}