Added unconstrained mhe

ChaserMHE.m

@@ -158,6 +158,18 @@
         end
 
 
+        function [Aeq, beq] = DynamicConstraintMatrices(obj, vector, horizon, tstep)
+            %{
+                Ax + Bu = x_k+1
+                Ax - Ix_k+1 = -B*u
+                [A ... -I]x = -B*u
+            %}
+            beq = [];
+            [A,B] = ARPOD_Benchmark.linearDynamics(tstep);
+            for i = 1:horizon-1
+                beq = [beq; -B*obj.window_control(:,i)];
+            end
+        end
         function ceq = setupDynamicConstraints(obj, vector, horizon, tstep)
             ceq = [];
             ceq = vector(1:6,:) - obj.window_states(:,1); % set x0 - xbar0 = 0;
@@ -212,6 +224,7 @@
 
             nonlcon = obj.setupEqualityConstraints(horizon, tstep);
             objective = ChaserMHE.quadraticCost(horizon, Q_cov, R_cov, obj.forget_factor);
+            %objective = ChaserMHE.loglikelihoodCost(horizon, Q_cov, R_cov, obj.forget_factor);
 
             x0 = obj.windowsToVector();
             A = [];

ChaserMHE_Unconstr.m

@@ -0,0 +1,108 @@
+classdef ChaserMHE_Unconstr
+    properties 
+        n_horizon
+        window_measurements = []; % these are observed in simulation
+        window_states %these are windows of states
+        window_control = [];
+        state
+
+    end
+    methods (Static)  
+        function modified_sense = senseModify(measurement)
+            [dim,i] = size(measurement);
+            modified_sense = measurement;
+            if dim == 2
+                modified_sense(3) = 0;
+            end
+        end
+    end
+    methods
+        function obj = init(obj, traj0, meas, n_horizon, tstep)
+            obj.n_horizon = n_horizon;
+
+            [A,B] = ARPOD_Benchmark.linearDynamics(tstep);
+            window_state = zeros(6,n_horizon);
+            window_state(:,1) = traj0;
+            for i = 2:n_horizon
+                window_state(:,i) = A*window_state(:,i-1);
+            end
+            obj.window_states = window_state;
+            obj.window_measurements = ChaserMHE.senseModify(meas);
+        end
+
+        function vector = windowsToVector(obj, n_horizon)
+            vector = [];
+            for i = 1:n_horizon
+                vector = [vector; obj.window_states(:,i)];
+            end
+        end
+        function obj = vectorToWindows(obj, vector, n_horizon)
+            for i = 1:n_horizon
+                obj.window_states(:,i) = vector(6*(i-1)+1:6*(i-1)+6);
+            end
+        end
+        function obj = windowShift(obj, meas, control, tstep)
+            [A,B] = ARPOD_Benchmark.linearDynamics(tstep);
+            [dim, horizon] = size(obj.window_measurements);
+
+
+            if horizon < obj.n_horizon
+                obj.window_measurements = [obj.window_measurements, ChaserMHE_Unconstr.senseModify(meas)];
+                obj.window_control = [obj.window_control, control];
+                obj.window_states(:,horizon+1) = A*obj.window_states(:,horizon) + B*control;
+
+            else
+                obj.window_measurements = [obj.window_measurements(:,2:obj.n_horizon), ChaserMHE.senseModify(meas)];
+                obj.window_states = [obj.window_states(:,2:obj.n_horizon), A*obj.window_states(:,obj.n_horizon) + B*control];
+
+                [dim, n] = size(obj.window_control);
+                obj.window_control = [obj.window_control(:,2:n), control];
+            end
+        end
+
+        function cost = quadraticCost(obj, Q, R, n_horizon, tstep)
+            [A,B] = ARPOD_Benchmark.linearDynamics(tstep);
+            function e = Cost(x)
+                e = 0;
+                for i = 1:n_horizon
+                    if i ~= n_horizon
+                        traj = x(6*(i-1)+1:6*(i-1)+6);
+                        next_traj = x(6*(i)+1:6*(i)+6);
+                        term = next_traj - (A*traj + B*obj.window_control(:,i));
+                        e = e + term.'*Q*term;
+                    end
+                    traj = x(6*(i-1)+1:6*(i-1)+6);
+                    traj_meas = ARPOD_Sensing.measure(traj);
+                    term = obj.window_measurements(:,i) - traj_meas;
+                    if obj.window_measurements(3,i) == 0
+                        term(3) = 0;
+                    end
+                    e = e + term.'*R*term;
+                end
+            end
+            cost = @Cost;
+        end
+        function obj = optimize(obj, Q_cov, R_cov, tstep)
+            [dim, horizon] = size(obj.window_measurements);
+
+            objective = obj.quadraticCost(Q_cov,R_cov,horizon,tstep);
+
+            x0 = obj.windowsToVector(horizon);
+
+            xstar = fminunc(objective, x0);
+            %
+            [dim, horizon] = size(xstar);
+            if horizon == 0
+                error("Optimization went wrong!!");
+            end
+            disp(objective(xstar))
+            obj = obj.vectorToWindows(xstar, horizon);
+        end
+        function obj = estimate(obj, control, measurement, Q_cov, R_cov,tstep, phase) %ducktyping for all state estimators
+            obj = obj.windowShift(measurement, control, tstep);
+            obj = obj.optimize(Q_cov, R_cov, tstep);
+
+            obj.state = obj.window_states(:,length(obj.window_states));
+        end     
+    end
+end

benchmark/MPC_Benchmark.m

@@ -24,11 +24,11 @@
 
 
 %initial parameters
-%traj = [-1;-0;0;0.01;0.01;0.1];
-traj = [-6;-6;6;0.01;0.0001;0.0001];
-%traj = [-10;0;0;-0.01;0.001;0.001];
+%traj = [-1;-0;0;0.01;0.01;0.001];
+%traj = [-6;-6;6;0.01;0.0001;0.0001];
+traj = [-5;5;5;-0.01;0.001;0.001];
 %total_time = ARPOD_Benchmark.t_e; %equate the benchmark duration to eclipse time
-total_time = 2000;
+total_time = 500;
 tstep = 5; % update every second
 phase = ARPOD_Benchmark.calculatePhase(traj,0);
 
@@ -62,7 +62,7 @@
         2: Particle Filter
         3: Moving Horizon Estimator
 %}
-stateEstimatorOption = 3;
+stateEstimatorOption = 4;
 
 %Setting up State Estimator Q and R matrices
 %{
@@ -98,7 +98,7 @@
     % tunable estimators
     seQ = diag(zeros(1,6)+process_noise);
     seR = 1e5*diag([0.001,0.001,0.01]);
-else
+elseif stateEstimatorOption == 3
     %moving horizon estimator
     stateEstimator = ChaserMHE;
 
@@ -116,11 +116,22 @@
         seQ = process_noise.^(-1)*diag([1,1,1,1,1,1]); %arbitrarily large
     end
     seR = diag([1e3, 1e3, 1e2]);
-end
+elseif stateEstimatorOption == 4 %MHE unconstr
+    stateEstimator = ChaserMHE_Unconstr;
+    mhe_horizon = 10;
+
+
+    if process_noise == 0
+        seQ = 1e20*diag([1,1,1,1,1,1]); %arbitrarily large
+    else
+        seQ = process_noise.^(-1)*diag([1,1,1,1,1,1]); %arbitrarily large
+    end
+    sense = ARPOD_Benchmark.sensor(traj, @() [0;0;0], phase);
+    stateEstimator = stateEstimator.init(traj, sense, mhe_horizon, tstep);
 
-%setting up gaussian noise models
-scale_sensorNoise = 1;
-scale_dynamicNoise = 0.000000001;
+elseif stateEstimatorOption == 5 %MHE EKF
+elseif stateEstimatorOption == 6 %MHE EKF unconstr
+end
 
 %initialize statistics for graph
 stats = ARPOD_Statistics;