inverted_pendulum_time_domain.m

Objective-C source code icon inverted_pendulum_time_domain.m — Objective-C source code, 2 kB (2059 bytes)

File contents

function [Kemean,Kestd,Xs,TT,KeM]=inverted_pendulum_time_domain(a,P)
global L K M c Delta delta tf

X        = zeros(4,M,L+1);      % Predefining X 
Torq     = zeros(2,M,L+1);      % Predefining Torq 
KeM      = zeros(M,2,4);        % Predefining Estimated K on M paths
n1       = zeros(M,L+1);        % Noise on accelaration          
n2       = zeros(M,L+1);        % Noise on T1
n3       = zeros(M,L+1);        % Noise on T2

X(:,:,1) = zeros(4,M,1);        % Setting Initial Condition



for j=1:L
    
    Winc1 = zeros(M,1); 
    Winc2 = zeros(M,1); 
    Winc3 = zeros(M,1); 
    
    for k=1:K
        
        dW1    = a*randn(M,1)./sqrt(delta);
        dW2    = c*randn(M,1)./sqrt(delta);
        dW3    = c*randn(M,1)./sqrt(delta);
        Winc1  = Winc1+dW1;
        Winc2  = Winc2+dW2;
        Winc3  = Winc3+dW3;
         
    end

    n1(:,j+1)=dW1;
    n2(:,j+1)=dW2;
    n3(:,j+1)=dW3;
    
    Torq(:,:,j)  = -P.K*X(:,:,j)+[Winc2';Winc3'];
    X(:,:,j+1)   = X(:,:,j)+(P.A*X(:,:,j)+P.B*(Torq(:,:,j))+P.W*Winc1')*Delta;
    
    if j==L
       Torq(:,:,j+1)  = -P.K*X(:,:,j+1)+[Winc2';Winc3']; 
    end
    
end


XX    = [squeeze(X(1,:,:));squeeze(X(2,:,:));squeeze(X(3,:,:));squeeze(X(4,:,:))];
T1    = squeeze(Torq(1,:,:));
T2    = squeeze(Torq(2,:,:));



%% Sampling Data

tsim          = 0:Delta:tf;
dt            = 0.01;
sampletimes   = 0:dt:tf;


Fsx = griddedInterpolant({1:size(X,1),1:size(X,2),tsim},X);
Fs1 = griddedInterpolant({1:size(T1,1),tsim},T1);
Fs2 = griddedInterpolant({1:size(T2,1),tsim},T2);
Fsn1= griddedInterpolant({1:size(n1,1),tsim},n1);

Xs  = Fsx({1:size(X,1),1:size(X,2),sampletimes});  
Ts1 = Fs1({1:size(T1,1),sampletimes});
Ts2 = Fs2({1:size(T2,1),sampletimes});
ns1 = Fsn1({1:size(n1,1),sampletimes});

%% Estimating K matrix
for i=1:M
    
    TT         = [Ts1(i,:);Ts2(i,:)];
    Kei        = -TT/squeeze(Xs(:,i,:));
    KeM(i,:,:) = Kei; 
end

Kemean         = squeeze(mean(KeM));
Kestd          = squeeze(std(KeM));