CODEWITHSUNDEEP
matlab
abatea
abatea

Reputation: 81

MATLAB, equationsToMatrix for nonlinear equations

After finding equations of motion using the Symbolic Toolbox (R2016b, Windows), I have the following form:

M(q)*qddot = b(q,qdot) + u

M and b were found using equationsToMatrix.

Now, I need to separate b into Coriolis and Potential terms such that

M(q)*qddot + C(q,qdot)*qdot + G(q) = u

It would be extremely convenient if I could apply

[C,G] = equationsToMatrix(b,qdot)

but unfortunately it will not factor out qdot when b is nonlinear. I do not care (and in fact it is necessary) that C is a function of q and qdot, even after factoring out the vector qdot. I have tried coeffs and factor without results.

Upvotes: 1

Views: 1448

Answers (2)

Levar
Levar

Reputation: 1

Dude, I would like to thank you from the bottom of my heart for your solution to your problem. I needed to solve the same problem with the equations of motion of a body with 6 degrees of freedom, by hand it would have been very long. I divided the system of original differential equations into matrices and then multiplied again and everything matches the original ones.

Here is an example of my steps as I obtained each matrix:

q = [x; y; z; phi; theta; psi]
qdot = [x_dot;y_dot;z_dot;phi_dot;theta_dot;psi_dot]
qddot = [x_ddot;y_ddot;z_ddot;phi_ddot;theta_ddot;psi_ddot]

% initial equations of motion
eqns = transpose([eqddx, eqddy, eqddz, eqddphi, eqddtheta, eqddpsi])

%Mass and inertia matrix (you can also use the matlab function)
[MM, zbytek] = equationsToMatrix(eqns, qddot)

%Coriolis force and drag force matrix
[C_G, zbytek2] = equationsToMatrix_abatea(-1*zbytek, qdot)

%my some inputs in differential equations
inputs = [Thrust; Tau_phi; Tau_theta; Tau_psi];

%Matrix for inputs L and gravity Q
[L, Q] = equationsToMatrix_abatea(-1*zbytek2, inputs)
Q = -1*Q;

Multiplication for comparison

vychozi = expand(eqns)
roznasobeni = expand( MM*qddot + C_G*qdot + Q == L*inputs)

Upvotes: -1

abatea
abatea

Reputation: 81

Posting my own solution so there can be at least one answer... This function works, but it is not heavily tested. It works exactly as I suggested in my original question. Feel free to rename it so it doesn't conflict with the MATLAB builtin.

function [A,b] = equationsToMatrix( eq, x )
%EQUATIONSTOMATRIX equationsToMatrix for nonlinear equations
%   factors out the vector x from eq such that eq = Ax + b
%   eq does not need to be linear in x
%   eq must be a vector of equations, and x must be a vector of symbols

assert(isa(eq,'sym'), 'Equations must be symbolic')
assert(isa(x,'sym'), 'Vector x must be symbolic')

n = numel(eq);
m = numel(x);

A = repmat(sym(0),n,m);

for i = 1:n % loop through equations
    [c,p] = coeffs(eq(i),x); % separate equation into coefficients and powers of x(1)...x(n)
    for k = 1:numel(p) % loop through found powers/coefficients
        for j = 1:m % loop through x(1)...x(n)
            if has(p(k),x(j))
                % transfer term c(k)*p(k) into A, factoring out x(j)
                A(i,j) = A(i,j) + c(k)*p(k)/x(j);
                break % move on to next term c(k+1), p(k+1)
            end
        end
    end
end

b = simplify(eq - A*x,'ignoreanalyticconstraints',true); % makes sure to fully cancel terms

end

Upvotes: 2

Related Questions