Commit fe4ac7c6 authored by Jakob Gabriel's avatar Jakob Gabriel
Browse files

minor bug fix in misc.ss.planTrajectory.m

parent f69f944e
...@@ -47,7 +47,6 @@ if optArgs.method == "Bernstein" ...@@ -47,7 +47,6 @@ if optArgs.method == "Bernstein"
end end
%W2 = expm( sys.A * (t.Discrete - t1 ) ) * cumInt( invPhi_t1 * sys.B * sys.B' * invPhi_t1', t, t0, t.name); %W2 = expm( sys.A * (t.Discrete - t1 ) ) * cumInt( invPhi_t1 * sys.B * sys.B' * invPhi_t1', t, t0, t.name);
%% Chen^3
if optArgs.method == "Chen3" if optArgs.method == "Chen3"
Phi_t_t0 = expm(sys.A * ( t.Symbolic - t.lower )); Phi_t_t0 = expm(sys.A * ( t.Symbolic - t.lower ));
Phi_t1_t = expm(sys.A * (t.upper - t.Symbolic ) ); Phi_t1_t = expm(sys.A * (t.upper - t.Symbolic ) );
...@@ -57,10 +56,7 @@ if optArgs.method == "Chen3" ...@@ -57,10 +56,7 @@ if optArgs.method == "Chen3"
Wt0t1 = Wt0t.at(t.upper); Wt0t1 = Wt0t.at(t.upper);
u = - sys.B' * Phi_t1_t' / Wt0t1 * ( Phi_t_t0.at(t.upper) * x0 - x1 ) * optArgs.weight; u = - sys.B' * Phi_t1_t' / Wt0t1 * ( Phi_t_t0.at(t.upper) * x0 - x1 ) * optArgs.weight;
x = Phi_t_t0 * x0 - Phi_t_t1 * Wt0t / Wt0t1 * ( Phi_t_t0.at(t.upper) * x0 - x1 ); x = Phi_t_t0 * x0 - Phi_t_t1 * Wt0t / Wt0t1 * ( Phi_t_t0.at(t.upper) * x0 - x1 );
end elseif optArgs.method == "Chen1"
%% Chen^1
if optArgs.method == "Chen1"
%% compute the gramian controllability matrix %% compute the gramian controllability matrix
Phi_t_t0 = expm(sys.A * ( t.Symbolic - t.lower )); Phi_t_t0 = expm(sys.A * ( t.Symbolic - t.lower ));
Phi_t0_t = expm(sys.A * ( t.lower - t.Symbolic)); % = inv( Phi_t_t0 ) Phi_t0_t = expm(sys.A * ( t.lower - t.Symbolic)); % = inv( Phi_t_t0 )
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment