diff --git a/sw/simulator/scilab/q3d/test_polynomial.sce b/sw/simulator/scilab/q3d/test_polynomial.sce index 63f8b9ef3e..82a2f4fd14 100644 --- a/sw/simulator/scilab/q3d/test_polynomial.sce +++ b/sw/simulator/scilab/q3d/test_polynomial.sce @@ -1,144 +1,33 @@ clear() -// -// traj is a n_compo*n_order*n_sample vector -// n_compo components -// n_order succesive time derivatives -// n_sample time values -// -function poly_display_traj(time, traj) - - [n_compo, n_order, n_sample] = size(traj); - - for compo=1:n_compo - for order=1:n_order - subplot(n_order, n_compo, compo+(order-1)*n_compo); - plot2d(time, matrix(traj(compo,order,:), n_sample, 1)); - xtitle(sprintf('$X^{%d}_{%d}$', order-1, compo)); - end - end - -endfunction +// a0 + a1(t-t0) + ... + an(t-tO)^n +coefs = -// -// compute the values of a set of polynomials along a time vector -// -// -function [traj] = poly_gen_traj(time, coefs) - - [n_comp, n_order, n_coef] = size(coefs); - - traj = zeros(n_comp, n_coef/2, length(time)); - for compo=1:n_comp - for order=1:n_order - for i=1:length(time) - traj(compo, order, i) = ... - poly_compute_val(matrix(coefs(compo,order, :),1,n_coef), time(1), time(i)); - end - end - end - -endfunction -// -// compute v = a_{n}*(t-t_0)^{n} + a_{n-1}*(t-t_0)^{n-1} + ... + a_{0} -// -// -function [v] = poly_compute_val(coefs, t0, t) - dt = t-t0; - v = coefs($); - for i=1:length(coefs)-1 - v = v * dt; - v = v + coefs(length(coefs)-i); // assume coef(1) = a_0 - end - -endfunction +if 0 +exec('q3d_utils.sci'); +exec('q3d_polynomials.sci'); +exec('q3d_diff_flatness.sci'); +exec('q3d_fdm.sci'); +exec('q3d_display.sci'); - -// -// compute coefficients for a set of polynomials -// having bond values b0 and b1 -// -// -function [coefs] = poly_get_coef_from_bound(time, b0,b1) - - [n_comp, n_order] = size(b0); - n_coef = 2*n_order - coefs = zeros(n_comp, n_order, n_coef); - - // refer to paper for notations - - for compo=1:n_comp - - // invert of the top left corner block - invA1 = zeros(n_order, n_order); - for i=1:n_order - invA1(i,i) = 1/Arr(i-1,i-1); - end - // first half of the coefficients - coefs(compo, 1, 1:n_order) = (invA1*b0(compo,:)')'; - - // bottom right block : triangular - A3 = zeros(n_order, n_order); - dt = time($) - time(1); - for i=1:n_order - for j=i:n_order - A3(i,j) = Arr(i-1,j-1) * dt^(j-1); - end - end - // bottom left block - A4 = zeros(n_order, n_order); - for i=1:n_order - for j=1:n_order - A4(i,j) = Arr(i-1,j-1+n_order) * dt^(j-1+n_order); - end - end - coefs(compo, 1, n_order+1:2*n_order) = ... - (inv(A4)*(b1(compo,:)' - A3*matrix(coefs(compo,1, 1:n_order), n_order, 1)))'; - // fill in the coefficients for the succesive time derivatives - for order=2:n_order - for pow=1:2*n_order-order - coefs(compo, order, pow+1) = Arr(order-1,pow-1+order)*coefs(compo, 1, pow+order); - end - end - end - -endfunction - - -// -// Arrangement -// -// -function [akn] = Arr(k,n) - akn = factorial(n)/factorial(n-k); -endfunction - - -b0 = [1 0 0 0; 1 0 0 0]; -b1 = [2 0 0 0;-1 0 0 0]; -t0 = 1; +b0 = [0 1 0]; +b1 = [2 0 0]; +t0 = 0; t1 = 2; dt = 0.01; time = t0:dt:t1; -if 1 + [coefs] = poly_get_coef_from_bound(time, b0, b1); -[traj] = poly_gen_traj(time, coefs); +[fo_traj] = poly_gen_traj(time, coefs); + + +set("current_figure",0); clf(); -poly_display_traj(time, traj); +f=get("current_figure"); +f.figure_name="Flat Outputs Trajectory"; +poly_display_traj(time, fo_traj); end - -if 0 -coefs = [0 1 0 ]; -time = 0:dt:1; -[traj] = poly_gen_traj(time, coefs); - -clf(); -poly_display_traj(time, traj); -end - - -// 1 0 0 10 -15 6 \ No newline at end of file