Add EKF

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

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.

inc/Matrix.h

@@ -10,6 +10,7 @@
 #include <initializer_list>
 #include <iostream>
 #include <vector>
+#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<double>& 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<double>& 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

inc/Model.h

@@ -29,16 +29,23 @@ namespace PCOE {
 
         using DynamicArray::operator[];
     };
-
+    
     class StateVector final : public DynamicArray<double> {
     public:
         using DynamicArray::DynamicArray;
 
         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<double>;
 
+
         /**
          * Initializes the model with the given parameters.
          *
@@ -133,14 +141,99 @@ 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,
                                       const state_type& x,
                                       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<MessageId> inputs;
         std::vector<MessageId> outputs;
+
+
     };
 }
 

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 <cmath>
+#include <vector>
+
+#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<UData> 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

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
      **/