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?
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.
Related
Consider the following problem:
I am now in the third part of this question. I wrote the vectorial loop equations (q=teta2, x=teta3 and y=teta4):
fval(1,1) = r2*cos(q)+r3*cos(x)-r4*cos(y)-r1;
fval(2,1) = r2*sin(q)+r3*sin(x)-r4*sin(y);
I have these 2 functions, and all variables except x and y are given. I found the roots with help of this video.
Now I need to plot graphs of q versus x and q versus y when q is at [0,2pi] with delta q of 2.5 degree. What should I do to plot the graphs?
Below is my attempt so far:
function [fval,jac] = lorenzSystem(X)
%Define variables
x = X(1);
y = X(2);
q = pi/2;
r2 = 15
r3 = 50
r4 = 45
r1 = 40
%Define f(x)
fval(1,1)=r2*cos(q)+r3*cos(x)-r4*cos(y)-r1;
fval(2,1)=r2*sin(q)+r3*sin(x)-r4*sin(y);
%Define Jacobian
jac = [-r3*sin(X(1)), r4*sin(X(2));
r3*cos(X(1)), -r4*cos(X(2))];
%% Multivariate NR
%Initial conditions:
X0 = [0.5;1];
maxIter = 50;
tolX = 1e-6;
X = X0;
Xold = X0;
for i = 1:maxIter
[f,j] = lorenzSystem(X);
X = X - inv(j)*f;
err(:,i) = abs(X-Xold);
Xold = X;
if (err(:,i)<tolX)
break;
end
end
Please take a look at my solution below, and study how it differs from your own.
function [th2,th3,th4] = q65270276()
[th2,th3,th4] = lorenzSystem();
hF = figure(); hAx = axes(hF);
plot(hAx, deg2rad(th2), deg2rad(th3), deg2rad(th2), deg2rad(th4));
xlabel(hAx, '\theta_2')
xticks(hAx, 0:pi/3:2*pi);
xticklabels(hAx, {'$0$','$\frac{\pi}{3}$','$\frac{2\pi}{3}$','$\pi$','$\frac{4\pi}{3}$','$\frac{5\pi}{3}$','$2\pi$'});
hAx.TickLabelInterpreter = 'latex';
yticks(hAx, 0:pi/6:pi);
yticklabels(hAx, {'$0$','$\frac{\pi}{6}$','$\frac{\pi}{3}$','$\frac{\pi}{2}$','$\frac{2\pi}{3}$','$\frac{5\pi}{6}$','$\pi$'});
set(hAx, 'XLim', [0 2*pi], 'YLim', [0 pi], 'FontSize', 16);
grid(hAx, 'on');
legend(hAx, '\theta_3', '\theta_4')
end
function [th2,th3,th4] = lorenzSystem()
th2 = (0:2.5:360).';
[th3,th4] = deal(zeros(size(th2)));
% Define geometry:
r1 = 40;
r2 = 15;
r3 = 50;
r4 = 45;
% Define the residual:
res = #(q,X)[r2*cosd(q)+r3*cosd(X(1))-r4*cosd(X(2))-r1; ... Δx=0
r2*sind(q)+r3*sind(X(1))-r4*sind(X(2))]; % Δy=0
% Define the Jacobian:
J = #(X)[-r3*sind(X(1)), r4*sind(X(2));
r3*cosd(X(1)), -r4*cosd(X(2))];
X0 = [acosd((45^2-25^2-50^2)/(-2*25*50)); 180-acosd((50^2-25^2-45^2)/(-2*25*45))]; % Accurate guess
maxIter = 500;
tolX = 1e-6;
for idx = 1:numel(th2)
X = X0;
Xold = X0;
err = zeros(maxIter, 1); % Preallocation
for it = 1:maxIter
% Update the guess
f = res( th2(idx), Xold );
X = Xold - J(Xold) \ f;
% X = X - pinv(J(X)) * res( q(idx), X ); % May help when J(X) is close to singular
% Determine convergence
err(it) = (X-Xold).' * (X-Xold);
if err(it) < tolX
break
end
% Update history
Xold = X;
end
% Unpack and store θ₃, θ₄
th3(idx) = X(1);
th4(idx) = X(2);
% Update X0 for faster convergence of the next case:
X0 = X;
end
end
Several notes:
All computations are performed in degrees.
The specific plotting code I used is less interesting, what matters is that I defined all θ₂ in advance, then looped over them to find θ₃ and θ₄ (without recursion, as was done in your own implementation).
The initial guess (actually, analytical solution) for the very first case (θ₂=0) can be found by solving the problem manually (i.e. "on paper") using the law of cosines. The solver also works for other guesses, but you might need to increase maxIter. Also, for certain guesses (e.g. X(1)==X(2)), the Jacobian is ill-conditioned, in which case you can use pinv.
If my computation is correct, this is the result:
Find the error as a function of n, where the error is defined as the difference between two the voltage from the Fourier series (vF (t)) and the value from the ideal function (v(t)), normalized to the maximum magnitude (Vm ):
I am given this prompt where Vm = 1 V. Below this line is the code which I have written.
I am trying to write a function to solve this question: Plot the error versus time for n=3,n=5,n=10, and n=50. (10points). What does it look like I am doing incorrectly?
clc;
close all;
clear all;
% define the signal parameters
Vm = 1;
T = 1;
w0 = 2*pi/T;
% define the symbolic variables
syms n t;
% define the signal
v1 = Vm*sin(4*pi*t/T);
v2 = 2*Vm*sin(4*pi*t/T);
% evaluate the fourier series integral
an1 = 2/T*int(v1*cos(n*w0*t),0,T/2) + 2/T*int(v2*cos(n*w0*t),T/2,T);
bn1 = 2/T*int(v1*sin(n*w0*t),0,T/2) + 2/T*int(v2*sin(n*w0*t),T/2,T);
a0 = 1/T*int(v1,0,T/2) + 1/T*int(v2,T/2,T);
% obtain C by substituting n in c[n]
nmax = 100;
n = 1:nmax;
a = subs(an1);
b = subs(bn1);
% define the time vector
ts = 1e-2; % ts is sampling the
t = 0:ts:3*T-ts;
% directly plot the signal x(t)
t1 = 0:ts:T-ts;
v1 = Vm*sin(4*pi*t1/T).*(t1<=T/2);
v2 = 2*Vm*sin(4*pi*t1/T).*(t1>T/2).*(t1<T);
v = v1+v2;
x = repmat(v,1,3);
% Now fourier series reconstruction
N = [3];
for p = 1:length(N)
for i = 1:length(t)
for k = N(p)
x(k,i) = a(k)*cos(k*w0*t(i)) + b(k)*sin(k*w0*t(i));
end
% y(k,i) = a0+sum(x(:,i)); % Add DC term
end
end
z = a0 + sum(x);
figure(1);
plot(t,z);
%Percent error
function [per_error] = percent_error(measured, actual)
per_error = abs(( (measured - actual) ./ 1) * 100);
end
The purpose of the forum is helping with specific technical questions, not doing your homework.
I have 2 features which I expand to contain all possible combinations of the two features under order 6. When I do MATLAB's fminunc, it returns a weight vector where all elements are 0.
The dataset is here
clear all;
clc;
data = load("P2-data1.txt");
m = length(data);
para = 0; % regularization parameter
%% Augment Feature
y = data(:,3);
new_data = newfeature(data(:,1), data(:,2), 3);
[~, n] = size(new_data);
betas1 = zeros(n,1); % initial weights
options = optimset('GradObj', 'on', 'MaxIter', 400);
[beta_new, cost] = fminunc(#(t)(regucostfunction(t, new_data, y, para)), betas1, options);
fprintf('Cost at theta found by fminunc: %f\n', cost);
fprintf('theta: \n');
fprintf(' %f \n', beta_new); % get all 0 here
% Compute accuracy on our training set
p_new = predict(beta_new, new_data);
fprintf('Train Accuracy after feature augmentation: %f\n', mean(double(p_new == y)) * 100);
fprintf('\n');
%% the functions are defined below
function g = sigmoid(z) % running properly
g = zeros(size(z));
g=ones(size(z))./(ones(size(z))+exp(-z));
end
function [J,grad] = regucostfunction(theta,x,y,para) % CalculateCost(x1,betas1,y);
m = length(y); % number of training examples
J = 0;
grad = zeros(size(theta));
hyp = sigmoid(x*theta);
err = (hyp - y)';
grad = (1/m)*(err)*x;
sum = 0;
for k = 2:length(theta)
sum = sum+theta(k)^2;
end
J = (1/m)*((-y' * log(hyp) - (1 - y)' * log(1 - hyp)) + para*(sum) );
end
function p = predict(theta, X)
m = size(X, 1); % Number of training examples
p = zeros(m, 1);
index = find(sigmoid(theta'*X') >= 0.5);
p(index,1) = 1;
end
function out = newfeature(X1, X2, degree)
out = ones(size(X1(:,1)));
for i = 1:degree
for j = 0:i
out(:, end+1) = (X1.^(i-j)).*(X2.^j);
end
end
end
data contains 2 columns of rows followed by a third column of 0/1 values.
The functions used are: newfeature returns the expanded features and regucostfunction computes the cost. When I did the same approach with the default features, it worked and I think the problem here has to do with some coding issue.
I compute the regression map of a time series A(t) on a field B(x,y,t) in the following way:
A=1:10; %time
B=rand(100,100,10); %x,y,time
rc=nan(size(B,1),size(B,2));
for ii=size(B,1)
for jj=1:size(B,2)
tmp = cov(A,squeeze(B(ii,jj,:))); %covariance matrix
rc(ii,jj) = tmp(1,2); %covariance A and B
end
end
rc = rc/var(A); %regression coefficient
Is there a way to vectorize/speed up code? Or maybe some built-in function that I did not know of to achieve the same result?
In order to vectorize this algorithm, you would have to "get your hands dirty" and compute the covariance yourself. If you take a look inside cov you'll see that it has many lines of input checking and very few lines of actual computation, to summarize the critical steps:
y = varargin{1};
x = x(:);
y = y(:);
x = [x y];
[m,~] = size(x);
denom = m - 1;
xc = x - sum(x,1)./m; % Remove mean
c = (xc' * xc) ./ denom;
To simplify the above somewhat:
x = [x(:) y(:)];
m = size(x,1);
xc = x - sum(x,1)./m;
c = (xc' * xc) ./ (m - 1);
Now this is something that is fairly straightforward to vectorize...
function q51466884
A = 1:10; %time
B = rand(200,200,10); %x,y,time
%% Test Equivalence:
assert( norm(sol1-sol2) < 1E-10);
%% Benchmark:
disp([timeit(#sol1), timeit(#sol2)]);
%%
function rc = sol1()
rc=nan(size(B,1),size(B,2));
for ii=1:size(B,1)
for jj=1:size(B,2)
tmp = cov(A,squeeze(B(ii,jj,:))); %covariance matrix
rc(ii,jj) = tmp(1,2); %covariance A and B
end
end
rc = rc/var(A); %regression coefficient
end
function rC = sol2()
m = numel(A);
rB = reshape(B,[],10).'; % reshape
% Center:
cA = A(:) - sum(A)./m;
cB = rB - sum(rB,1)./m;
% Multiply:
rC = reshape( (cA.' * cB) ./ (m-1), size(B(:,:,1)) ) ./ var(A);
end
end
I get these timings: [0.5381 0.0025] which means we saved two orders of magnitude in the runtime :)
Note that a big part of optimizing the algorithm is assuming you don't have any "strangeness" in your data, like NaN values etc. Take a look inside cov.m to see all the checks that we skipped.
Introduction
I am using Matlab to simulate some dynamic systems through numerically solving systems of Second Order Ordinary Differential Equations using ODE45. I found a great tutorial from Mathworks (link for tutorial at end) on how to do this.
In the tutorial the system of equations is explicit in x and y as shown below:
x''=-D(y) * x' * sqrt(x'^2 + y'^2)
y''=-D(y) * y' * sqrt(x'^2 + y'^2) + g(y)
Both equations above have form y'' = f(x, x', y, y')
Question
However, I am coming across systems of equations where the variables can not be solved for explicitly as shown in the example. For example one of the systems has the following set of 3 second order ordinary differential equations:
y double prime equation
y'' - .5*L*(x''*sin(x) + x'^2*cos(x) + (k/m)*y - g = 0
x double prime equation
.33*L^2*x'' - .5*L*y''sin(x) - .33*L^2*C*cos(x) + .5*g*L*sin(x) = 0
A single prime is first derivative
A double prime is second derivative
L, g, m, k, and C are given parameters.
How can Matlab be used to numerically solve a set of second order ordinary differential equations where second order can not be explicitly solved for?
Thanks!
Your second system has the form
a11*x'' + a12*y'' = f1(x,y,x',y')
a21*x'' + a22*y'' = f2(x,y,x',y')
which you can solve as a linear system
[x'', y''] = A\f
or in this case explicitly using Cramer's rule
x'' = ( a22*f1 - a12*f2 ) / (a11*a22 - a12*a21)
y'' accordingly.
I would strongly recommend leaving the intermediate variables in the code to reduce chances for typing errors and avoid multiple computation of the same expressions.
Code could look like this (untested)
function dz = odefunc(t,z)
x=z(1); dx=z(2); y=z(3); dy=z(4);
A = [ [-.5*L*sin(x), 1] ; [.33*L^2, -0.5*L*sin(x)] ]
b = [ [dx^2*cos(x) + (k/m)*y-g]; [-.33*L^2*C*cos(x) + .5*g*L*sin(x)] ]
d2 = A\b
dz = [ dx, d2(1), dy, d2(2) ]
end
Yes your method is correct!
I post the following code below:
%Rotating Pendulum Sym Main
clc
clear all;
%Define parameters
global M K L g C;
M = 1;
K = 25.6;
L = 1;
C = 1;
g = 9.8;
% define initial values for theta, thetad, del, deld
e_0 = 1;
ed_0 = 0;
theta_0 = 0;
thetad_0 = .5;
initialValues = [e_0, ed_0, theta_0, thetad_0];
% Set a timespan
t_initial = 0;
t_final = 36;
dt = .01;
N = (t_final - t_initial)/dt;
timeSpan = linspace(t_final, t_initial, N);
% Run ode45 to get z (theta, thetad, del, deld)
[t, z] = ode45(#RotSpngHndl, timeSpan, initialValues);
%initialize variables
e = zeros(N,1);
ed = zeros(N,1);
theta = zeros(N,1);
thetad = zeros(N,1);
T = zeros(N,1);
V = zeros(N,1);
x = zeros(N,1);
y = zeros(N,1);
for i = 1:N
e(i) = z(i, 1);
ed(i) = z(i, 2);
theta(i) = z(i, 3);
thetad(i) = z(i, 4);
T(i) = .5*M*(ed(i)^2 + (1/3)*L^2*C*sin(theta(i)) + (1/3)*L^2*thetad(i)^2 - L*ed(i)*thetad(i)*sin(theta(i)));
V(i) = -M*g*(e(i) + .5*L*cos(theta(i)));
E(i) = T(i) + V(i);
end
figure(1)
plot(t, T,'r');
hold on;
plot(t, V,'b');
plot(t,E,'y');
title('Energy');
xlabel('time(sec)');
legend('Kinetic Energy', 'Potential Energy', 'Total Energy');
Here is function handle file for ode45:
function dz = RotSpngHndl(~, z)
% Define Global Parameters
global M K L g C
A = [1, -.5*L*sin(z(3));
-.5*L*sin(z(3)), (1/3)*L^2];
b = [.5*L*z(4)^2*cos(z(3)) - (K/M)*z(1) + g;
(1/3)*L^2*C*cos(z(3)) + .5*g*L*sin(z(3))];
X = A\b;
% return column vector [ed; edd; ed; edd]
dz = [z(2);
X(1);
z(4);
X(2)];