Adding source.

MATLAB/+Observers/ExtendedKalmanFilter.m

@@ -0,0 +1,113 @@
+classdef ExtendedKalmanFilter < Observers.Observer
+% ExtendedKalmanFilter   Class implementing the extended Kalman filter
+% algorithm
+%
+% This class implements the extended Kalman filter algorithm. It accepts a
+% model of the explicit discrete time-invariant form:
+%   x(k+1) = stateEqn(x(k),u(k),dt) + N(0,Q)
+%     y(k) = outputEqn(x(k),u(k)) = N(0,R)
+% where N is the standard normal distribution with zero mean and covariance
+% Q or R. This class implements the Observer interface.
+%
+% The state and output Jacobian matrices must also be provided. These are
+% functions, both taking as input x and u.
+%
+% ExtendedKalmanFilter Methods:
+%   initialize(EKF,t0,x0,u0) - Initialize filter given initial time, state,
+%   and inputs
+%   estimate(EKF,t,u,z) - Update the state and outpute estimates given new
+%   input and output data.
+%   getStateEstimate(EKF) - Return a state estimate structure with mean and
+%   covariance.
+%
+% See also Observers.Observer, Observers.KalmanFilter,
+% Observers.UnscentedKalmanFilter
+%
+% Copyright (c) 2016 United States Government as represented by the
+% Administrator of the National Aeronautics and Space Administration.
+% No copyright is claimed in the United States under Title 17, U.S.
+% Code. All Other Rights Reserved.
+
+	properties
+		stateEqn    % State update equation (function handle)
+		outputEqn   % Output equation (function handle)
+		xjacobian   % State Jacobian matrix
+		zjacobian   % Output Jacobian matrix
+		x           % State estimate
+		z           % Output estimate
+		P           % State covariance matrix estimate
+		Q           % Process noise covariance matrix
+		R           % Sensor noise covariance matrix
+    end
+
+	methods
+
+		function EKF = ExtendedKalmanFilter(stateEqn,outputEqn,xjacobian,zjacobian,modelVariance,sensorVariance)
+			% ExtendedKalmanFilter   Constructor
+            %   Construct an EKF given the state equation, output equation,
+            %   state Jacobian, output Jacobian, and the process and sensor
+            %   noise covariance matrices.
+            EKF.stateEqn = stateEqn;
+			EKF.outputEqn = outputEqn;
+			EKF.xjacobian = xjacobian;
+			EKF.zjacobian = zjacobian;
+			EKF.Q = modelVariance;
+			EKF.R = sensorVariance;
+			EKF.P = zeros(size(modelVariance));
+			EKF.initialized = false;
+        end
+
+
+		function initialize(EKF,t0,x0,u0)
+            % initialize   Initialize the EKF given initial time, states,
+            % and inputs
+            EKF.t = t0;
+            EKF.x = x0;
+            EKF.u = u0;
+            EKF.z = EKF.outputEqn(x0,u0);
+			EKF.initialized = true;
+        end
+
+
+		function estimate(EKF,t,u,z)
+            % estimate   Update the state estimate for the current time
+            % given inputs and outputs
+
+            % Ensure EKF is initialized
+            if ~EKF.initialized
+                error('ExtendedKalmanFilter must be initialized first!');
+            end
+
+			deltaT = newT-EKF.t;
+
+            % Predict
+            Fk = EKF.xjacobian(EKF.x,EKF.u);
+            xkk1 = EKF.stateEqn(EKF.x,EKF.u,deltaT);
+            Pkk1 = Fk*EKF.P*Fk'+EKF.Q;
+
+            % Update
+			Hk = EKF.zjacobian(xkk1,u);
+			yk = z-EKF.outputEqn(xkk1,u);
+			Sk = Hk*Pkk1*Hk'+EKF.R;			
+			Kk = Pkk1*Hk'/Sk;
+			EKF.x = xkk1+Kk*yk;
+			EKF.P = (eye(length(EKF.x))-Kk*Hk)*Pkk1;
+			EKF.z = EKF.outputEqn(EKF.x,u);
+
+			% Update time, inputs
+            EKF.u = u;
+            EKF.t = t;
+        end
+
+
+        function stateEstimate = getStateEstimate(EKF)
+            % getStateEstimate  Return a structure with mean and covariance
+            %   Returns the current state estimate as a structure with
+            %   fields 'mean' and 'covariance'.
+            stateEstimate.mean = EKF.x;
+            stateEstimate.covariance = EKF.P;
+        end
+
+	end
+end
+

MATLAB/+Observers/KalmanFilter.m

@@ -0,0 +1,125 @@
+classdef KalmanFilter < Observers.Observer
+% KalmanFilter   Class implementing Kalman filter algorithm.
+%
+% This class implements the Kalman filter algorithm. It maintains the
+% current state and output estimates, and updates these when new data is
+% provided. The system model is given in the form of the state space
+% matrices A, B, C, D, and process and sensor noise covariance matrices Q
+% and R:
+%   x(k+1) = A x(k) + B u(k) + N(0,Q)
+%     y(k) = C x(k) + D u(k) + N(0,R)
+% where N is the standard normal distribution with zero mean and covariance
+% Q or R.
+%
+% This class implements the Observer interface.
+%
+% KalmanFilter Methods:
+%   initialize(KF,t0,x0,u0) - Initialize filter given initial time, state,
+%   and inputs
+%   estimate(KF,t,u,z) - Update the state and outpute estimates given new
+%   input and output data.
+%   getStateEstimate(KF) - Return a state estimate structure with mean and
+%   covariance.
+%
+% Note that although these methods take time as an input, the state space
+% matrices are time-invariant and so must be defined with respect to the
+% desired sampling rate. The filter only keeps track of the current time
+% given to it and does not use time in any calculations.
+%
+% See also Observers.Observer, Observers.ExtendedKalmanFilter,
+% Observers.UnscentedKalmanFilter
+%
+% Copyright (c) 2016 United States Government as represented by the
+% Administrator of the National Aeronautics and Space Administration.
+% No copyright is claimed in the United States under Title 17, U.S.
+% Code. All Other Rights Reserved.
+
+	properties
+		A;  % State equation matrix (multiplied by x)
+		B;  % State equation matrix (multipied by u)
+		C;  % Output equation matrix (multiplied by x)
+		D;  % Output equation matrix (multipied by u)
+		x;  % State estimate
+		z;  % Output estimate
+		P;  % State covariance matrix estimate
+		Q;  % Process noise covariance matrix
+		R;  % Sensor noise covariance matrix
+    end
+
+	methods
+
+		function KF = KalmanFilter(A,B,C,D,Q,R)
+            % KalmanFilter  Constructor
+            %   Construct a KalmanFilter object given state-space matrices
+            %   A, B, C, D, Q, R.
+
+            % Set all matrices
+			KF.A = A;
+			KF.B = B;
+			KF.C = C;
+			KF.D = D;
+			KF.Q = Q;
+			KF.R = R;
+
+            % Set initialized flag
+            KF.initialized = false;
+        end
+
+
+        function initialize(KF,t0,x0,u0)
+            % initialize  Initialize the Kalman filter
+            %   Initialize the KalmaFilter object given initial time,
+            %   state, and inputs.
+
+            % Intialize time, state, input, output, and state covariance
+            KF.t = t0;
+            KF.x = x0;
+            KF.z = KF.C*x0 + KF.D*u0;
+            KF.u = u0;
+            KF.P = KF.Q;
+
+            % Set initialized flag
+            KF.initialized = true;
+        end
+
+
+		function estimate(KF,t,u,z)
+            % estimate  Perform an estimation step of the Kalman filter
+            %   Given new inputs and outptus, udpate the state and output
+            %   estimates.
+
+            % Ensure KF is initialized
+            if ~KF.initialized
+                error('KalmanFilter must be initialized first!');
+            end
+
+            % Predict state and covariance
+            xkk1 = KF.A*KF.x + KF.B*KF.u;
+            Pkk1 = KF.A*KF.P*KF.A' + KF.Q;
+
+            % Update state, outputs, and covariance
+			zk = z - (KF.C*KF.x + KF.D*u);
+			Sk = KF.C*Pkk1*KF.C' + KF.R;		
+			Kk = Pkk1*KF.C'/Sk;
+			KF.x = xkk1 + Kk*zk;
+			KF.P = (eye(length(KF.x)) - Kk*KF.C)*Pkk1;
+			KF.z = KF.C*KF.x + KF.D*u;
+
+            % Update time and inputs
+            KF.t = t;
+            KF.u = u;
+        end
+
+
+        function stateEstimate = getStateEstimate(KF)
+            % getStateEstimate  Return a structure with mean and covariance
+            %   Returns the current state estimate as a structure with
+            %   fields 'mean' and 'covariance'.
+
+            % Construct the data structure
+            stateEstimate.mean = KF.x;
+            stateEstimate.covariance = KF.P;
+        end
+
+	end
+end

MATLAB/+Observers/Observer.m

@@ -0,0 +1,41 @@
+classdef Observer < handle
+% Observer   Interface class for state observer objects
+% Abstract class for creating Observer objects that perform model-based
+% state estimation. Subclass constructors should accept model equations
+% and/or parameters.
+%
+% Observer Methods:
+%   initialize(obj,t0,x0,u0) - Initialize filter given initial time, state,
+%   and inputs
+%   estimate(obj,t,u,z) - Update the state and outpute estimates given new
+%   input and output data.
+%   getStateEstimate(obj) - Return a state estimate structure with mean and
+%   covariance. 
+%
+% See also Observers.KalmanFilter, Observers.ExtendedKalmanFilter,
+% Observers.ParticleFilter, Observers.UnscentedKalmanFilter
+%
+% Copyright (c) 2016 United States Government as represented by the
+% Administrator of the National Aeronautics and Space Administration.
+% No copyright is claimed in the United States under Title 17, U.S.
+% Code. All Other Rights Reserved.
+
+    properties
+        t;            % Last updated time
+        initialized;  % Flag describing whether observer is initialized
+        u;            % Input at time t
+    end
+
+    methods (Abstract)
+
+        % Given initial state and inputs, initialize observer
+        initialize(obj,t,x,u)
+
+        % Perform an estimation step given new time, inputs, and outputs
+        estimate(obj,t,u,z)
+
+        % Get state estimate
+        getStateEstimate(obj)
+    end
+
+end