Skip to content

Commit

Permalink
Fixed MHE. Everything Up to Date and Fixed Now!
Browse files Browse the repository at this point in the history
  • Loading branch information
@dangthanhan507
dangthanhan507 committed Aug 18, 2022
1 parent 69bfbdd commit f4e08c6
Show file tree
Hide file tree
Showing 4 changed files with 67 additions and 43 deletions.
8 changes: 8 additions & 0 deletions ARPOD_Statistics.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand Down
27 changes: 19 additions & 8 deletions ChaserMHE.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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


Expand Down Expand Up @@ -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
Expand All @@ -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);

Expand Down
15 changes: 9 additions & 6 deletions ChaserPF.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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
Expand Down
60 changes: 31 additions & 29 deletions benchmark/MPC_Benchmark.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
Graph should give the estimated trajectory
TODO: Run MHE
Tests: different phases
changing covairances from the true to measure adaptability
Expand All @@ -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);

%{
Expand All @@ -62,7 +62,7 @@
2: Particle Filter
3: Moving Horizon Estimator
%}
stateEstimatorOption = 2;
stateEstimatorOption = 3;

%Setting up State Estimator Q and R matrices
%{
Expand All @@ -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

Expand Down Expand Up @@ -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]).';
Expand All @@ -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
Expand Down

0 comments on commit f4e08c6

Please sign in to comment.