Added APF

ChaserMHE.m

@@ -93,7 +93,7 @@
                 window_state(:,i) = A*window_state(:,i-1);
             end
             obj.window_measError = 100*ones(3,n_horizon);
-            obj.window_stateError = zeros(6,n_horizon);
+            obj.window_stateError = 100*ones(6,n_horizon);
             obj.window_states = window_state;
             obj.forget_factor = forget_factor;
 
@@ -146,7 +146,7 @@
 
                 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];
-                obj.window_stateError = [obj.window_stateError(:,2:obj.n_horizon), zeros(6,1)];
+                obj.window_stateError = [obj.window_stateError(:,2:obj.n_horizon), 100*ones(6,1)];
 
                 state_meas = ARPOD_Benchmark.sensor(obj.window_states(:,obj.n_horizon),@() [0;0;0], 2);
                 error = ChaserMHE.senseModify(meas) - state_meas;
@@ -221,7 +221,8 @@
             lb = ones(length(x0),1) - Inf;
             ub = ones(length(x0),1) + Inf;
 
-            options = optimoptions(@fmincon, 'Algorithm', 'sqp', 'MaxIterations', 2000, 'ConstraintTolerance', 1e-12);
+            %options = optimoptions(@fmincon, 'Algorithm', 'interior-point', 'MaxIterations', 2000, 'ConstraintTolerance', 1e-12, "EnableFeasibilityMode",true, "HonorBounds", true);
+            options = optimoptions(@fmincon, 'Algorithm', 'sqp', 'MaxIterations', 2000, 'ConstraintTolerance', 1e-6);
             xstar = fmincon(objective, x0, A, b, Aeq, beq, lb, ub, nonlcon, options);
             %
             [dim, horizon] = size(xstar);

ChaserMPC.m

@@ -221,9 +221,10 @@
             end
             [n,m] = size(A);
             b = zeros(n,1);
-            options = optimoptions(@quadprog, 'Algorithm', 'active-set');
+            %options = optimoptions(@quadprog, 'Algorithm', 'active-set');
             x0 = zeros(length(H),1);
-            final = quadprog(H,f,A,b,Aeq,beq,lb,ub,x0,options);
+            %final = quadprog(H,f,A,b,Aeq,beq,lb,ub,x0,options);
+            final = quadprog(H,f,A,b,Aeq,beq,lb,ub);
 
             [xs,us] = ChaserMPC.extractOptVector(final, n_horizon);
         end

ChaserMPC_APF.m

@@ -0,0 +1,30 @@
+classdef ChaserMPC_APF
+    methods (Static)
+        function cost = setupOriginalAPF(traj, obstacle, kO, KO)
+            xyz = [traj(1); traj(2); traj(3)];
+            xyzO = obstacle(1:3,:);
+            dO = obstacle(4,:);
+            dist = norm(xyz-xyzO);
+            if dist <= dO
+                cost = 0.5*KO*(norm(xyz-xyzO) - kO * dO).^2;
+            else
+                cost = 0;
+            end
+        end
+        function costF = costAPF(Q,R, n_horizon, obstacles, KO, kO)
+            %{
+                Obstacle: (x,y,z, distance)
+            %}
+            [H,f] = ChaserMPC.setupQuadraticCost(Q,R,n_horizon);
+            %APF
+            [dim,n_obstacles] = size(obstacles);
+            function cost = costfun(traj)
+                cost = 0;
+                for i = 1:n_obstacles
+                    cost = cost + ChaserMPC_APF.setupOriginalAPF(traj, obstacles(:,i), kO, KO);
+                end
+            end
+            costF = @costfun;
+        end
+    end
+end

benchmark/MPC_Benchmark.m

@@ -11,14 +11,13 @@
 
 
         Tests: different phases
-                changing covairances from the true to measure adaptability
-                (eps)
-                try different initial conditions
+                try init MHE w/ EKF
                 think of different noise profiles for sensors
-                
-                record MPC for mission characteristics
-                root MSE
 
+                add process noise 5% (std: 0.05)
+
+                use NERM equations w/ elliptical orbit
+                
                 
 %}
 rng(1);
@@ -28,7 +27,7 @@
 %traj = [-0.2;-0.2;0.2;0.001;0.001;0.001];
 traj = [-10;-10;10;0.01;0.0001;0.0001];
 %total_time = ARPOD_Benchmark.t_e; %equate the benchmark duration to eclipse time
-total_time = 5000;
+total_time = 120;
 tstep = 5; % update every second
 phase = ARPOD_Benchmark.calculatePhase(traj,0);
 
@@ -96,7 +95,7 @@
     %moving horizon estimator
     stateEstimator = ChaserMHE;
 
-    mhe_horizon = 20;
+    mhe_horizon = 10;
     forget_factor = 1;
 
 
@@ -148,6 +147,7 @@
 
     % use the u to simulate next step of the chaser spacecraft
     traj = ARPOD_Benchmark.nextStep(traj.',u,tstep,noiseQ,1);
+
     % simulate sensor reading
     sense = ARPOD_Benchmark.sensor(traj,noiseR,phase);
 

benchmark/Obstacle_Benchmark.m

@@ -0,0 +1,168 @@
+%{
+    MPC Benchmark Script
+    --------------------
+        Description:
+        ------------
+            Given choice of Particle Filter, Extended Kalman Filter, and
+            Moving Horizon Estimator, follow and estimate trajectory using
+            Moving Horizon Estimator.
+
+            Graph should give the estimated trajectory 
+
+
+        Tests: different phases
+                changing covairances from the true to measure adaptability
+                (eps)
+                try different initial conditions
+                think of different noise profiles for sensors
+                
+                record MPC for mission characteristics
+                root MSE
+
+                
+%}
+rng(1);
+
+
+%initial parameters
+%traj = [-0.2;-0.2;0.2;0.001;0.001;0.001];
+traj = [-10;-10;10;0.01;0.0001;0.0001];
+%total_time = ARPOD_Benchmark.t_e; %equate the benchmark duration to eclipse time
+total_time = 100;
+tstep = 5; % update every second
+phase = ARPOD_Benchmark.calculatePhase(traj,0);
+
+%MPC parameters
+mpc_horizon = 50;
+scale_mpcQ = 1;
+scale_mpcR = 10;
+mpc_Q = scale_mpcQ*[1,0,0,0,0,0;
+        0,1,0,0,0,0;
+        0,0,1,0,0,0;
+        0,0,0,100,0,0;
+        0,0,0,0,100,0;
+        0,0,0,0,0,100];
+mpc_R = scale_mpcR*eye(3);
+
+%{
+    Model Predictive Control choice:
+    --------------------------------
+        1. Linear MPC --> quadprog
+        2. Nonlinear MPC --> fmincon
+%}
+mpc_choice = 1;
+if mpc_choice == 2
+    mpc = ChaserNLMPC;
+end
+
+%{
+    State estimator choice:
+    -----------------------
+        1: Extended Kalman Filter
+        2: Particle Filter
+        3: Moving Horizon Estimator
+%}
+stateEstimatorOption = 3;
+
+%Setting up State Estimator Q and R matrices
+%{
+
+    For EKF, Q is the process covariance and R is sensor covariance.
+    For PF, it's the same as EKF
+    For MHE, Q is the cost weight matrix for state error. R is for meas
+    error.
+
+%}
+if (stateEstimatorOption == 1)
+    %EKF
+    stateEstimator = ChaserEKF;
+    stateEstimator = stateEstimator.initEKF(traj, 1e-10*eye(6)); %really trust initial estimate.
+
+    % tunable parameters
+    seQ = 1e-20*diag([1,1,1,1,1,1]);
+    seR = diag([0.001,0.001,0.01]);
+elseif (stateEstimatorOption == 2)
+    %PF
+    ess_threshold = 700;
+    n_particles = 1000;
+
+    stateEstimator = ChaserPF;
+    stateEstimator = stateEstimator.initPF(traj.', 1e-50*eye(6), n_particles, ess_threshold);
+
+    % tunable estimators
+    seQ = 1e-20*diag([1,1,1,1,1,1]);
+    seR = diag([0.001,0.001,0.01]);
+else
+    %moving horizon estimator
+    stateEstimator = ChaserMHE;
+
+    mhe_horizon = 20;
+    forget_factor = 1;
+
+
+    sense = ARPOD_Benchmark.sensor(traj, @() [0;0;0], phase);
+    stateEstimator = stateEstimator.initMHE(traj,sense,mhe_horizon, forget_factor, tstep);
+
+
+    seQ = 1e20*diag([1,1,1,1,1,1]); %arbitrarily large
+    seR = diag([1e3, 1e3, 1e2]);
+end
+
+%setting up gaussian noise models
+scale_sensorNoise = 1;
+scale_dynamicNoise = 0.000000001;
+
+%initialize statistics for graph
+stats = ARPOD_Statistics;
+stats = stats.initBenchmark(traj);
+
+estTraj = traj;
+
+
+for i = tstep:tstep:total_time
+    disp(i)
+    phase = ARPOD_Benchmark.calculatePhase(traj,false);
+    %calculate the next "u" using MPC
+
+    %vary the process noise depending based on benchmark
+    %benchmark doesn't consider process noise :{
+    %only considers sensor noise and varies it depending on phase
+    if phase == 1
+        noiseQ = @() [0;0;0;0;0;0];
+        noiseR = @() mvnrnd([0;0;0], [0.001, 0.001, 0.01]).';
+    elseif phase == 2
+        noiseQ = @() [0;0;0;0;0;0];
+        noiseR = @() 10*mvnrnd([0;0;0], [0.001, 0.001, 0.01]).';
+    else
+        noiseQ = @() [0;0;0;0;0;0];
+        noiseR = @() mvnrnd([0;0;0], [0.001, 0.001, 1e-5]).';
+    end
+
+
+    if mpc_choice == 1 %linear
+        u = ChaserMPC.controlMPC(traj,mpc_Q,mpc_R,mpc_horizon,tstep,ARPOD_Benchmark.m_c,phase);
+    else %nonlinear 
+        mpc = mpc.controlMPC(traj, mpc_Q, mpc_R, tstep, mpc_horizon, ARPOD_Benchmark.m_c, phase);
+        u = mpc.warm_u(:,1);
+    end
+
+    % use the u to simulate next step of the chaser spacecraft
+    traj = ARPOD_Benchmark.nextStep(traj.',u,tstep,noiseQ,1);
+
+    % simulate sensor reading
+    sense = ARPOD_Benchmark.sensor(traj,noiseR,phase);
+
+    %given sensor reading read EKF
+    stateEstimator = stateEstimator.estimate(u, sense, seQ, seR, tstep, phase);
+    estTraj = stateEstimator.state;
+
+    stats = stats.updateBenchmark(u, ARPOD_Benchmark.m_c, traj, estTraj, tstep, phase);
+
+    if sqrt(traj(1).^2 + traj(2).^2 + traj(3).^2) < 0.001
+        disp("Docked Early")
+        break
+    end
+end
+
+%graph results
+stats.graphLinear(pi/3,pi/3);