diff --git a/ChaserMHE.m b/ChaserMHE.m index ac90dc0..970c251 100644 --- a/ChaserMHE.m +++ b/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); diff --git a/ChaserMPC.m b/ChaserMPC.m index af1eacd..20b6e10 100644 --- a/ChaserMPC.m +++ b/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 diff --git a/ChaserMPC_APF.m b/ChaserMPC_APF.m new file mode 100644 index 0000000..23342ee --- /dev/null +++ b/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 \ No newline at end of file diff --git a/benchmark/MPC_Benchmark.m b/benchmark/MPC_Benchmark.m index 8eb9634..d5fdf64 100644 --- a/benchmark/MPC_Benchmark.m +++ b/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); diff --git a/benchmark/Obstacle_Benchmark.m b/benchmark/Obstacle_Benchmark.m new file mode 100644 index 0000000..39e925b --- /dev/null +++ b/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); \ No newline at end of file