Skip to content

Commit

Permalink
Updated LQR and code to visualize example benchmark
Browse files Browse the repository at this point in the history
  • Loading branch information
@dangthanhan507
dangthanhan507 committed Jul 27, 2022
1 parent 1339b08 commit 3e889e8
Show file tree
Hide file tree
Showing 6 changed files with 120 additions and 39 deletions.
47 changes: 35 additions & 12 deletions ARPOD_Benchmark.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,14 @@
t_e = 14400; % eclipse time (in seconds)
t_f = 43200; % total mission duration (in seconds)
rho_r = 1; % maximum distance for range measurements (1 km)
%rho_r = 50;
rho_d = 0.1; % docking phase initial radius (0.1 km)
%rho_d = 25;
m_t = 2000; % mass of target (2000 kg)
m_c = 500; % mass of chaser (500 kg)
mu = 398600.4; %earth's gravitational constant (in km^3/s^2)
a = 42164; % semi-major axis of GEO (42164 km)
%a = 1000;
Vbar = 5 * 10.^(-5); % max closing velocity while docking (in km/s)
theta = 60; % LOS Constraint angle (in degrees)
c = [-1;0;0]; % LOS cone direction
Expand All @@ -19,13 +22,13 @@
end
methods (Static)
function phase = calculatePhase(traj, reached)
norm = traj(:,1:3);
norm = traj(1:3,:);
norm = sqrt(norm.^2);
if (reached == 0)
if (norm > 1)
if (norm > ARPOD_Benchmark.rho_r)
% ARPOD phase 1: Rendezvous w/out range
phase = 1;
elseif (norm > 0.1)
elseif (norm > ARPOD_Benchmark.rho_d)
% ARPOD phase 2: Rendezvous with range
phase = 2;
else
Expand All @@ -41,36 +44,56 @@
if (options == 1)
% discrete control input
u0 = @(t) u;
[ts, trajs] = nonlinearChaserDynamics.simulateMotion(traj0, ARPOD_Benchmark.a, u0, timestep, timestep);
traj = trajs(2,:);
[ts, trajs] = nonlinearChaserDynamics.simulateMotion(traj0, ARPOD_Benchmark.a, u0, timestep, 0);
traj = trajs(length(ts),:);
elseif (options == 2)
% discrete impulsive control input
% To be Implemented
disp('it is not implemented!. What are you doing?');
elseif (options == 3)
% continuous control input
[ts, trajs] = nonlinearChaserDynamics.simulateMotion(traj0, ARPOD_Benchmark.a, u,timestep,timestep);
traj = trajs(2,:);
[ts, trajs] = nonlinearChaserDynamics.simulateMotion(traj0, ARPOD_Benchmark.a, u,timestep, 0);
traj = trajs(length(ts),:);
end
traj = traj + noise();
end
function sensor = sensor(state, noise, options)
if (options == 1)
%phase 1: only using bearing measurements
sensor = ARPOD.measure(state);
sensor = ARPOD_Sensing.measure(state);
sensor = sensor(1:2,:);
w = noise();
v = w(1:2,:);
elseif (options == 2)
%phase 2: bearing measurements + range measurement
sensor = ARPOD.measure(state);
sensor = ARPOD_Sensing.measure(state);
v = noise();
elseif (options == 3)
%phase 3: same as phase 2
sensor = ARPOD.measure(state);
sensor = ARPOD_Sensing.measure(state);
v = noise();
elseif (options == 4)
%phase 4: relative phase 2 to partner spacecraft
r = ARPOD_Benchmark.x_partner - [state(1);state(2);state(3)]; % relative position to partner spacecraft
sensor = ARPOD.measure(r);
sensor = ARPOD_Sensing.measure(r);
v = noise();
end
sensor = sensor + v;
end
function jacobian = jacobianSensor(state, options, r)
x = state(1);
y = state(2);
z = state(3);
if (options == 1)
jacobian = ARPOD_Sensing.jacobianMeasurement(x,y,z);
jacobian = jacobian(1:2,:);
elseif (options == 2)
jacobian = ARPOD_Sensing.jacobianMeasurement(x,y,z);
elseif (options == 3)
jacobian = ARPOD_Sensing.jacobianMeasurement(x,y,z);
elseif (options == 4)
jacobian = ARPOD_Sensing.jacobianPartner(x,y,z,r);
end
sensor = sensor + noise();
end
end
end
7 changes: 4 additions & 3 deletions ChaserEKF.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,12 +36,13 @@
state = A*state0 + B*u0;
cov = A*cov0*transpose(A) + systemCov;
end
function [state,cov] = estimate(state_t,cov_t,u_t,T,R,z_t,systemCov,measCov)
function [state,cov] = estimate(state_t,cov_t,u_t,T,R,z_t,systemCov,measCov, phase)
[state_pred, cov_pred] = ChaserEKF.prediction(state_t,cov_t,u_t,T,R,systemCov);
H = ARPOD_Sensing.jacobianMeasurement(state_pred(1),state_pred(2),state_pred(3)); %jacobian matrix
H = ARPOD_Benchmark.jacobianSensor(state_pred,phase, ARPOD_Benchmark.x_partner);
K_gain = cov_pred*transpose(H)*inv(H*cov_pred*transpose(H) + measCov);

state = state_pred + K_gain*(z_t-ARPOD_Sensing.measure(state_pred));
measure = ARPOD_Benchmark.sensor(state_pred, @() [0;0;0], phase);
state = state_pred + K_gain*(z_t-measure);
cov = (eye(6) - K_gain*H)*cov_pred;
end
end
Expand Down
28 changes: 10 additions & 18 deletions ChaserLQR.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,39 +2,31 @@
properties (Constant)
end
methods (Static)
function [A,B] = linearDynamics(R, T)
function [A,B] = linearDynamics(R)
mu_GM = 398600.4; %km^2/s^2;

n = sqrt(mu_GM / (R.^3) );
A = zeros(6,6);
B = zeros(6,3);
S = sin(n * T);
C = cos(n * T);

A(1,:) = [4-3*C,0,0,S/n,2*(1-C)/n,0];
A(2,:) = [6*(S-n*T),1,0,-2*(1-C)/n,(4*S-3*n*T)/n,0];
A(3,:) = [0,0,C,0,0,S/n];
A(4,:) = [3*n*S,0,0,C,2*S,0];
A(5,:) = [-6*n*(1-C),0,0,-2*S,4*C-3,0];
A(6,:) = [0,0,-n*S,0,0,C];
A(1:3,4:6) = eye(3);
A(4,1) = 3*n*n;
A(6,3) = -n*n;
A(5,4) = -2*n;
A(4,5) = 2*n;

B(1,:) = [(1-C)/(n*n),(2*n*T-2*S)/(n*n),0];
B(2,:) = [-(2*n*T-2*S)/(n*n),(4*(1-C)/(n*n))-(3*T*T/2),0];
B(3,:) = [0,0,(1-C)/(n*n)];
B(4,:) = [S/n,2*(1-C)/n, 0];
B(5,:) = [-2*(1-C)/n,(4*S/n) - (3*T),0];
B(6,:) = [0,0,S/n];
B(4:6,1:3) = eye(3);
end
function u = optimal_control(state, Q, R, R_orbit, timestep)
function u = optimal_control(state, Q, R, R_orbit)
% LQR does not care about any controller constraints imposed by
% ARPOD problem. LQR should be used to help test the state
% estimators.

% cannot solve HJB formualtion since a matrix required needs to
% solve Riccati Algebraic Equation

[A,B] = ChaserLQR.linearDynamics(R_orbit, timestep);
[K,S,e] = LQR(A,B,Q,R,zeros(3,3));
[A,B] = ChaserLQR.linearDynamics(R_orbit);
[K,S,e] = lqr(A,B,Q,R);
u = -K*state;
end
end
Expand Down
68 changes: 68 additions & 0 deletions benchmark/exampleBenchmark.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
N = 100;
x = [-1000,-2000,-3000,-100,-100,-100];
x_est = x.';
cov_t = eye(6);
tstep = 1;

trajTrue = zeros(N,6);
trajTrue(1,:) = x;

trajEst = zeros(N,6);
trajEst(1,:) = x;

noise = @() transpose(mvnrnd([0;0;0], [1,1,0.001], 1));

record_controlinput = zeros(3,N-1);
record_phases = zeros(1,N-1);

sensor_cov = 1000*[1,0,0;0,1,0;0,0,0.01];
process_cov = 1*eye(6);
for i = 2:N
disp(i)
phase = ARPOD_Benchmark.calculatePhase(x.', 0);
record_phases(:,i-1) = phase;

u_lqr = ChaserLQR.optimal_control(x_est, 1*eye(6), 10000*eye(3),ARPOD_Benchmark.a);

%u_lqr = [5;5;5];
record_controlinput(:,i-1) = u_lqr;
x = ARPOD_Benchmark.nextStep(x,u_lqr,tstep, @() [0,0,0,0,0,0], 1);
trajTrue(i,:) = x.';

%
meas = ARPOD_Benchmark.sensor(x,noise,phase);
if phase == 1
[x_est,cov_t] = ChaserEKF.estimate(x_est, cov_t, u_lqr,tstep,ARPOD_Benchmark.a, meas, process_cov, sensor_cov(1:2,1:2), phase);
else
[x_est,cov_t]= ChaserEKF.estimate(x_est, cov_t, u_lqr,tstep,ARPOD_Benchmark.a, meas, process_cov, sensor_cov, phase);
end
trajEst(i,:) = x_est.';
end

figure(1)
plot3(trajEst(:,1), trajEst(:,2), trajEst(:,3), '-r');
hold on
plot3(trajTrue(:,1), trajTrue(:,2), trajTrue(:,3), '-b');
hold off
title('Benchmark')
xlabel('x')
ylabel('y')
zlabel('z')
grid on

figure(2)
plot(linspace(1,N,N),trajEst(:,1),'-r')
hold on
plot(linspace(1,N,N),trajTrue(:,1),'-b')
hold off
title('x trajectory over time')
grid on

figure(3)
plot(linspace(1,N-1,N-1),sum(record_controlinput.^2), '-b')
title('Control Input L2-Norm')
grid on

figure (4)
plot(linspace(1,N-1,N-1),record_phases,'-b')
title("Phase")
7 changes: 2 additions & 5 deletions nonlinearChaserDynamics.m
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
% gravitational constant in km^2/s^2 from chance-constr MPC

classdef nonlinearChaserDynamics
properties (Constant)
mu_GM = 398600.4;
end
methods (Static)
function traj = ChaserMotion(t,traj0,R,u)
%{
Expand All @@ -29,7 +26,7 @@
zotzotzot hehe.
%}
%traj is [x,y,z,xdot,ydot,zdot]
mu_GM = nonlinearChaserDynamics.mu_GM;
mu_GM = ARPOD_Benchmark.mu;
x = traj0(1);
y = traj0(2);
z = traj0(3);
Expand Down Expand Up @@ -58,7 +55,7 @@
function [ts,trajs] = simulateMotion(traj0,R,u,T,timestep)
f = @(t,traj) nonlinearChaserDynamics.ChaserMotion(t,traj,R,u);
if timestep==0
[ts,trajs] = ode45(f,0:T, traj0);
[ts,trajs] = ode45(f,[0,T], traj0);
else
[ts,trajs] = ode45(f,0:timestep:T, traj0);
end
Expand Down
2 changes: 1 addition & 1 deletion testFolder/testEKF.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
estimatedTraj(:,1) = state0;
for i = 2:length(ts)
T = ts(i) - ts(i-1);
[est_state, est_cov] = ChaserEKF.estimate(estimatedTraj(:,i-1), estimatedTrajCov(:,:,i-1), [0;0;0], T, R, sense_data(:,i), 1*eye(6), 0.1*eye(3));
[est_state, est_cov] = ChaserEKF.estimate(estimatedTraj(:,i-1), estimatedTrajCov(:,:,i-1), [0;0;0], T, R, sense_data(:,i), 1*eye(6), 0.001*eye(3), 2);
estimatedTraj(:,i) = est_state;
estimatedTrajCov(:,:,i) = est_cov;
end
Expand Down

0 comments on commit 3e889e8

Please sign in to comment.