0

I am trying to simulate the rotation dynamics of a system. I am testing my code to verify that it's working using simulation, but I never recovered the parameters I pass to the model. In other words, I can't re-estimate the parameters I chose for the model.

I am using MATLAB for that and specifically ode45. Here is my code:

% Load the input-output data
[torque outputs] = DataLogs2(); 
u = torque;

% using the simulation data
Ixx = 1.00;
Iyy = 2.00;
Izz = 3.00;

x0          = [0; 0; 0];
Ts          = .02;
t           = 0:Ts:Ts * ( length(u) - 1 );
[ T, x ]    = ode45( @(t,x) rotationDyn( t, x, u(1+floor(t/Ts),:), Ixx, Iyy, Izz), t, x0 );

w           = x';
N           = length(w);
q           = 1;           % a counter for the A and B matrices

% The Algorithm
for k=1:1:N
    w_telda    = [   0      -w(3, k)    w(2,k); ...
                   w(3,k)       0      -w(1,k); ...
                  -w(2,k)    w(1,k)       0  ];

    if k == N        % to handle the problem of the last iteration
        w_dash(:,k)  = (-w(:,k))/Ts;
    else
        w_dash(:,k)  = (w(:,k+1)-w(:,k))/Ts;
    end

    a            = kron( w_dash(:,k)', eye(3) ) + kron( w(:,k)', w_telda );
    A(q:q+2,:)   = a;           % a 3N*9 matrix
    B(q:q+2,:)   = u(k,:)';     % a 3N*1 matrix   % u(:,k)

    q = q + 3;
end

% Forcing J to be diagonal. This is the case when we consider our quadcopter as two thin uniform 
% rods crossed at the origin with a point mass (motor) at the end of each.
A_new                 = [A(:, 1) A(:, 5) A(:, 9)];
vec_J_diag            = A_new\B;
J_diag                = diag([vec_J_diag(1), vec_J_diag(2), vec_J_diag(3)])
eigenvalues_J_diag    = eig(J_diag) 
error                 = norm(A_new*vec_J_diag - B)

where my dynamic model is defined as:

function [dw, y] = rotationDyn(t, w, tau, Ixx, Iyy, Izz, varargin)

    % The output equation
    y = [w(1); w(2); w(3)];

    % State equation
    % dw = (I^-1)*( tau - cross(w, I*w) );
    dw = [Ixx^-1 * tau(1) - ((Izz-Iyy)/Ixx)*w(2)*w(3);
          Iyy^-1 * tau(2) - ((Ixx-Izz)/Iyy)*w(1)*w(3);
          Izz^-1 * tau(3) - ((Iyy-Ixx)/Izz)*w(1)*w(2)];
    end

Practically, what this code should do, is to calculate the eigenvalues of the inertia matrix, J, i.e. to recover Ixx, Iyy, and Izz that I passed to the model at the very begining (1, 2 and 3), but all what I get is wrong results.

Is the problem with using ode45?

horchler
  • 18,384
  • 4
  • 37
  • 73
M.A.
  • 169
  • 1
  • 1
  • 18
  • Sorry for not knowing where the problem exactly is, but I spent many days looking for it with no luck – M.A. Oct 26 '15 at 13:47
  • Please provide some type of output so that we can better understand what you mean by "wrong results." Perhaps a plot of the results and a plot (or description) of what you think it should be. – Brian Goodwin Oct 26 '15 at 17:41
  • @brian, The pre-specified values of the three inertia `Ixx, Iyy, Izz` are 1.0, 2.0 and 3.0, but the estimated results I am getting are : [-2.855851586472459e-01, 2.086082382514133e-03, 6.027561523432094e-02] which are the eigenvalues of the `J_diag` matrix above in the code – M.A. Oct 27 '15 at 08:42

1 Answers1

0

Well the problem wasn't in the ode45 instruction, the problem is that in system identification one can create an n-1 samples-signal from an n samples-signal, thus the loop has to end at N-1 in the above code.

M.A.
  • 169
  • 1
  • 1
  • 18