diff --git a/ARPOD_Statistics.m b/ARPOD_Statistics.m index fe15430..6ecaefd 100644 --- a/ARPOD_Statistics.m +++ b/ARPOD_Statistics.m @@ -79,6 +79,7 @@ plot3(obj.trackTraj(1,:), obj.trackTraj(2,:), obj.trackTraj(3,:), 'r'); %draw estchaserTraj plot3(obj.trackEstTraj(1,:), obj.trackEstTraj(2,:), obj.trackEstTraj(3,:), 'b'); + %draw target position %draw chaserStart %draw chaserEnd @@ -102,6 +103,13 @@ xlabel("time (in seconds)") ylabel("Squared Error"); + figure(4) + plot(obj.trackTraj(1,:), obj.trackTraj(2,:), 'r'); + plot(obj.trackEstTraj(1,:), obj.trackEstTraj(2,:), 'b'); + title('Chaser Trajectory 2D') + xlabel("x"); + ylabel("y"); + return; end function totalMSE = getError(obj) diff --git a/ChaserMHE.m b/ChaserMHE.m index 0d6c226..ac90dc0 100644 --- a/ChaserMHE.m +++ b/ChaserMHE.m @@ -92,7 +92,7 @@ for i = 2:n_horizon window_state(:,i) = A*window_state(:,i-1); end - obj.window_measError = zeros(3,n_horizon); + obj.window_measError = 100*ones(3,n_horizon); obj.window_stateError = zeros(6,n_horizon); obj.window_states = window_state; obj.forget_factor = forget_factor; @@ -137,14 +137,24 @@ if num < obj.n_horizon obj.window_measurements = [obj.window_measurements, ChaserMHE.senseModify(meas)]; + obj.window_control = [obj.window_control, control]; + + obj.window_states(:,num+1) = A*obj.window_states(:,num) + B*control; + state_meas = ARPOD_Benchmark.sensor(obj.window_states(:,num+1),@() [0;0;0], 2); + obj.window_measError(:,num+1) = ChaserMHE.senseModify(meas) - state_meas; 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]; obj.window_stateError = [obj.window_stateError(:,2:obj.n_horizon), zeros(6,1)]; - obj.window_measError = [obj.window_measError(:,2:obj.n_horizon), zeros(3,1)]; + state_meas = ARPOD_Benchmark.sensor(obj.window_states(:,obj.n_horizon),@() [0;0;0], 2); + error = ChaserMHE.senseModify(meas) - state_meas; + obj.window_measError = [obj.window_measError(:,2:obj.n_horizon), error]; + + [dim, n] = size(obj.window_control); + obj.window_control = [obj.window_control(:,2:n), control]; end - obj.window_control = [obj.window_control, control]; end @@ -181,8 +191,8 @@ measE_ = vi; state_meas = ARPOD_Benchmark.sensor(state_, @() zeros(3,1), 2); - if meas(2) == 0 - state_meas(2) = 0; + if meas(3) == 0 + state_meas(3) = 0; end ceq = [ceq; state_meas + measE_ - meas]; % g(x_k) + w_k - y_k = 0 end @@ -209,20 +219,21 @@ Aeq = []; beq = []; lb = ones(length(x0),1) - Inf; - ub = -lb; + ub = ones(length(x0),1) + Inf; - options = optimoptions(@fmincon, 'Algorithm', 'sqp', 'MaxIterations', 1000, 'ConstraintTolerance', 1e-6); + options = optimoptions(@fmincon, 'Algorithm', 'sqp', 'MaxIterations', 2000, 'ConstraintTolerance', 1e-12); xstar = fmincon(objective, x0, A, b, Aeq, beq, lb, ub, nonlcon, options); % [dim, horizon] = size(xstar); if horizon == 0 error("Optimization went wrong!!"); end + disp(objective(xstar)) obj = obj.vectorToWindows(xstar); end %estimate (piecing it together) - function obj = estimate(obj, control, measurement, Q_cov, R_cov,tstep,phase) %ducktyping for all state estimators + 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); diff --git a/ChaserPF.m b/ChaserPF.m index 63a3669..52d37dd 100644 --- a/ChaserPF.m +++ b/ChaserPF.m @@ -11,7 +11,6 @@ rand = mvnrnd(mu,cov,1); end function indexes = sample_particles(weights) - disp(weights); [N,dim] = size(weights); indexes = randsample(N,N,true,weights.'); end @@ -36,13 +35,13 @@ %} ESS = 1/sum(obj.weights.^2); if (ESS < obj.ESS_Threshold) - %resample old_particles = obj.particles; - idx_particles = ChaserPF.sample_particles(obj.particles); - for i =1:obj.n_particles + idx_particles = ChaserPF.sample_particles(obj.weights); + for i = 1:obj.n_particles j = idx_particles(i); obj.particles(:,i) = old_particles(:,j); end + N = obj.n_particles; obj.weights = ones(N,1)/N; end end @@ -56,6 +55,7 @@ function obj = particlesUpdate(obj, measurement, Q_cov, R_cov, u, tstep, phase) [dim,N] = size(obj.particles); + idx_particles = ChaserPF.sample_particles(obj.weights); new_particles = zeros(dim,N); new_weights = zeros(N,1); @@ -65,12 +65,15 @@ R_cov = R_cov(1:2,1:2); end + [A,B] = ARPOD_Benchmark.linearDynamics(tstep); for i = 1:N j = idx_particles(i); - prop = ARPOD_Benchmark.nextStep(obj.particles(:,j), u,tstep,@() [0;0;0;0;0;0], 1); + %prop = ARPOD_Benchmark.nextStep(obj.particles(:,j), u,tstep,@() [0;0;0;0;0;0], 1); + prop = A*obj.particles(:,j) + B*u; + new_particles(:,i) = ChaserPF.sample_gaussian(prop,Q_cov); - estimated_measurement = ARPOD_Benchmark.sensor(prop, @() [0;0;0], phase); + estimated_measurement = ARPOD_Benchmark.sensor(new_particles(:,i), @() [0;0;0], phase); new_weights(i) = abs(mvnpdf(measurement, estimated_measurement, R_cov)); sum_weights = sum_weights + new_weights(i); end diff --git a/benchmark/MPC_Benchmark.m b/benchmark/MPC_Benchmark.m index 48ca9a2..8eb9634 100644 --- a/benchmark/MPC_Benchmark.m +++ b/benchmark/MPC_Benchmark.m @@ -9,7 +9,6 @@ Graph should give the estimated trajectory - TODO: Run MHE Tests: different phases changing covairances from the true to measure adaptability @@ -26,22 +25,23 @@ %initial parameters -traj = [-0.2;-0.2;0.2;0.001;0.001;0.001]; +%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 = 50; -tstep = 1; % update every second +total_time = 5000; +tstep = 5; % update every second phase = ARPOD_Benchmark.calculatePhase(traj,0); %MPC parameters -mpc_horizon = 100; -scale_mpcQ = 100; -scale_mpcR = 1; -mpc_Q = scale_mpcQ*[10,0,0,0,0,0; - 0,10,0,0,0,0; - 0,0,10,0,0,0; - 0,0,0,1,0,0; - 0,0,0,0,1,0; - 0,0,0,0,0,1]; +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); %{ @@ -62,7 +62,7 @@ 2: Particle Filter 3: Moving Horizon Estimator %} -stateEstimatorOption = 2; +stateEstimatorOption = 3; %Setting up State Estimator Q and R matrices %{ @@ -79,34 +79,32 @@ stateEstimator = stateEstimator.initEKF(traj, 1e-10*eye(6)); %really trust initial estimate. % tunable parameters - scale_Q = 1e-10; - scale_R = 1; - seQ = scale_Q*diag([1,1,1,0.1,0.1,0.1]); - seR = scale_R*diag([1,1,0.00001]); + seQ = 1e-20*diag([1,1,1,1,1,1]); + seR = diag([0.001,0.001,0.01]); elseif (stateEstimatorOption == 2) %PF - ess_threshold = 25; - n_particles = 30; + ess_threshold = 700; + n_particles = 1000; stateEstimator = ChaserPF; - stateEstimator = stateEstimator.initPF(traj.', 1e-10*eye(6), n_particles, ess_threshold); + stateEstimator = stateEstimator.initPF(traj.', 1e-50*eye(6), n_particles, ess_threshold); % tunable estimators - scale_Q = 1e-10; - scale_R = 1; - seQ = scale_Q*diag([1,1,1,0.1,0.1,0.1]); - seR = scale_R*diag([1,1,0.00001]); + 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 = 10; + 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 = diag([1e12,1e12,1e12,1e12,1e12,1e12]); %arbitrarily large + + seQ = 1e20*diag([1,1,1,1,1,1]); %arbitrarily large seR = diag([1e3, 1e3, 1e2]); end @@ -134,7 +132,7 @@ noiseR = @() mvnrnd([0;0;0], [0.001, 0.001, 0.01]).'; elseif phase == 2 noiseQ = @() [0;0;0;0;0;0]; - noiseR = @() mvnrnd([0;0;0], [0.001, 0.001, 0.01]).'; + 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]).'; @@ -153,12 +151,16 @@ % 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