From afc318a2e00ccb0d89d8d86954940f36c52d8efa Mon Sep 17 00:00:00 2001 From: an Date: Thu, 25 Aug 2022 23:01:02 -0700 Subject: [PATCH] Added unconstrained mhe --- ChaserMHE.m | 13 +++++ ChaserMHE_EKF.m | 0 ChaserMHE_EKF_Unconstr.m | 0 ChaserMHE_Unconstr.m | 108 ++++++++++++++++++++++++++++++++++++++ benchmark/MPC_Benchmark.m | 31 +++++++---- 5 files changed, 142 insertions(+), 10 deletions(-) create mode 100644 ChaserMHE_EKF.m create mode 100644 ChaserMHE_EKF_Unconstr.m create mode 100644 ChaserMHE_Unconstr.m diff --git a/ChaserMHE.m b/ChaserMHE.m index 970c251..137a571 100644 --- a/ChaserMHE.m +++ b/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 = []; diff --git a/ChaserMHE_EKF.m b/ChaserMHE_EKF.m new file mode 100644 index 0000000..e69de29 diff --git a/ChaserMHE_EKF_Unconstr.m b/ChaserMHE_EKF_Unconstr.m new file mode 100644 index 0000000..e69de29 diff --git a/ChaserMHE_Unconstr.m b/ChaserMHE_Unconstr.m new file mode 100644 index 0000000..00eebc3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/benchmark/MPC_Benchmark.m b/benchmark/MPC_Benchmark.m index 69577c3..e55e17c 100644 --- a/benchmark/MPC_Benchmark.m +++ b/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;