Benjamin2011.m

%% state estimator (One DOF Inverted Pendulum model)

% For MSc project on state estimation for humanoid robot,the basic approach
% that simplifies the humanoid robot to an inverted pendulum model, based
% on the paper ---¡¶State Estimation for Force-Controlled Humanoid Balance 
% using Simple Models in the Presence of Modeling Error¡·
% --- Benjamin J. Stephens  2011.
% By Jiacheng Gu on 2018/3/13
% ** means parameters need to be tuned

%% Initilization
clear;
clc;
close all;

NumOfStep = 1:800;
T = 25e-4;              % time step f = 400Hz
t = T * NumOfStep;  
g = 9.81;               % gravity
z_0 = 0.6;              % height of COM (centre of mass)
omega = sqrt(g / z_0);  % w 

% **
u_x = 0.0;                % ** u_x is an unknown external force (under Eq11)
x_offset = 0.02;        % ** x_offset is an unknown offset (under Eq9)

alpha = 0.01;           % ** amplitude of the known ground truth state
                        % trajectories generated by sinusoids
beta = 4 * pi;          % ** frequency
% **

% state trajectory
x = alpha * sin( beta * t);         % COM position (Eq22)
% x = normrnd(x,1e-8);
dx = alpha * beta * cos(beta * t);  % d(x) velocity (Eq23) 
% dx = normrnd(dx,1e-4);
                                    % position of COP (Eq24) 
x_c = x + 1 / omega^2 * (alpha * beta^2 * sin(beta * t) + u_x);
% x_c = normrnd(x_c,1e-8);                                
% COM measurement ,    y is the vector of measurement
y1 = x-x_offset;           % measured position of COM (Eq25)
% y1 = normrnd(y1,1e-8);
y2 = x_c;                  % measured position of COP (Eq26)
% y2 = normrnd(y2,1e-4);
%% Naive Estimator  (under EQ26)
A0 = [1 T 0; 
    omega^2 * T 1 -omega^2 * T;     % Eq3
    0 0 1];
B0 = [0 0 T]';                      % Eq4
C0_1  = [1 0 0];                    % Eq6
C0_2  = [0 0 1];                    % Eq7

x0 = [0 0 0]';                      % original x

u = diff(x_c)/T;                    % uk is input on timestep k
u = [u, u(end)];                    % here u is a vector wrt time t

Q = eye(3);                           % ** noise level in system dynamics
Q(1, 1) = 1e-8;
Q(2, 2) = 1e-4;
Q(3, 3) = 1e-4;

isUseCOP = 0;                       % 0 means not using COP, position only
R = eye(isUseCOP+1);                % ** noise level in measurement(table 2)
R(isUseCOP+1,isUseCOP+1) = 1e-5;
R(1,1) = 1e-5;
if isUseCOP
    y_S1 = [y1; y2]; %#ok<UNRCH>
    C0 = [C0_1;C0_2];
else
    y_S1 = y1;  
    C0 = C0_1;
end
[x_hat_array, ~, ~] = myEstimator(A0,B0,C0,u,y_S1,x0,Q,R);
x_Naive_estimated_S1   = x_hat_array(1,:);
dx_Naive_estimated_S1  = x_hat_array(2,:);
x_c_Naive_estimated_S1 = x_hat_array(3,:);

isUseCOP = 1;                       % 1 means using COP,both COP and position
R = eye(isUseCOP+1);                % ** noise level in measurement(table 2)
R(isUseCOP+1,isUseCOP+1) = 1e-5;
R(1,1) = 1e-5;
if isUseCOP
    y = [y1; y2]; 
    C0 = [C0_1;C0_2];
else
    y = y1;   %#ok<UNRCH>
    C0 = C0_1;
end

[x_hat_array, ~, ~] = myEstimator(A0,B0,C0,u,y,x0,Q,R);
x_Naive_estimated_S12   = x_hat_array(1,:);
dx_Naive_estimated_S12  = x_hat_array(2,:);
x_c_Naive_estimated_S12 = x_hat_array(3,:);
%% plot Naive estimator
% t = 1:length(t);
figure('units','normalized','outerposition',[0 0 1 1],'name','Naive State estimation')
subplot(4,1,1)
plot(t, x,'b')
hold on
grid on
plot(t, x_Naive_estimated_S1,'r--','linewidth',2)
plot(t, x_Naive_estimated_S12,'k:','linewidth',2)
legend('Ground Truth Position', 'S1', 'S12')
title('Position')

subplot(4,1,2)
plot(t, dx,'b')
hold on
grid on
plot(t, dx_Naive_estimated_S1,'r--','linewidth',2)
plot(t, dx_Naive_estimated_S12,'k:','linewidth',2)
legend('Ground Truth velocity', 'S1','S12')
title('Velocity')

subplot(4,1,3)
plot(t, x_c,'b')
hold on
grid on
plot(t, x_c_Naive_estimated_S1,'r--','linewidth',2)
plot(t, x_c_Naive_estimated_S12,'k:','linewidth',2)
legend('Ground Truth COP', 'S1','S12')
title('COP')

%% COM offset estimator
A1 = [1 T 0 0; 
    omega^2 * T 1 -omega^2 * T 0;   % Eq10
    0 0 1 0;
    0 0 0 1];
B1 = [0 0 T 0]';                      % Eq4
C1_1  = [1 0 0 1];                    % Eq18
C1_2  = [0 0 1 1];                    % Eq7

x0 = [0 0 0 0]';                      % original x

u = diff(x_c)/T;                    % uk is input on timestep k
u = [u, u(end)];                    % here u is a vector wrt time t

Q = 1e-8;                           % ** noise level in system dynamics

isUseCOP = 0;                       % 0 means not using COP, position only
R = eye(isUseCOP+1);                % ** noise level in measurement(table 2)
R(isUseCOP+1,isUseCOP+1) = 1e-5;
R(1,1) = 1e-5;
if isUseCOP
    y_S1 = [y1; y2]; %#ok<UNRCH>
    C1 = [C1_1;C1_2];
else
    y_S1 = y1;  
    C1 = C1_1;
end
[x_hat_array, ~, ~] = myEstimator(A1,B1,C1,u,y_S1,x0,Q,R);
x_COM_estimated_S1       = x_hat_array(1,:);
dx_COM_estimated_S1      = x_hat_array(2,:);
x_c_COM_estimated_S1     = x_hat_array(3,:);
delta_x_COM_estimated_S1 = x_hat_array(4,:);

isUseCOP = 1;                       % 1 means using COP,both COP and position
R = eye(isUseCOP+1);                % ** noise level in measurement(table 2)
R(isUseCOP+1,isUseCOP+1) = 1e-5;
R(1,1) = 1e-5;
if isUseCOP
    y = [y1; y2]; 
    C1 = [C1_1;C1_2];
else
    y = y1;   %#ok<UNRCH>
    C1 = C1_1;
end

[x_hat_array, ~, ~] = myEstimator(A1,B1,C1,u,y,x0,Q,R);
x_COM_estimated_S12       = x_hat_array(1,:);
dx_COM_estimated_S12      = x_hat_array(2,:);
x_c_COM_estimated_S12     = x_hat_array(3,:);
delta_x_COM_estimated_S12 = x_hat_array(4,:);

%% plot COM offset estimator
% t = 1:length(t);
figure('units','normalized','outerposition',[0 0 1 1],'name','Naive State estimation')
subplot(4,1,1)
plot(t, x,'b')
hold on
grid on
plot(t, x_COM_estimated_S1,'r--','linewidth',2)
plot(t, x_COM_estimated_S12,'k:','linewidth',2)
legend('Ground Truth Position', 'S1', 'S12')
title('Position')

subplot(4,1,2)
plot(t, dx,'b')
hold on
grid on
plot(t, dx_COM_estimated_S1,'r--','linewidth',2)
plot(t, dx_COM_estimated_S12,'k:','linewidth',2)
legend('Ground Truth velocity', 'S1','S12')
title('Velocity')

subplot(4,1,3)
plot(t, x_c,'b')
hold on
grid on
plot(t, x_c_COM_estimated_S1,'r--','linewidth',2)
plot(t, x_c_COM_estimated_S12,'k:','linewidth',2)
legend('Ground Truth COP', 'S1','S12')
title('COP')

subplot(4,1,4)
plot(t, x_offset * ones(length(t)),'b')
hold on
grid on
plot(t, delta_x_COM_estimated_S1,'r--','linewidth',2)
plot(t, delta_x_COM_estimated_S12,'k:','linewidth',2)
legend('Ground Truth COM offset', 'S1','S12')
title('COM offset')


%% External force estimator
A2 = [1 T 0 0; 
    omega^2 * T 1 -omega^2 * T T;   % Eq10
    0 0 1 0;
    0 0 0 1];
B2 = [0 0 T 0]';                      % Eq4
C2_1  = [1 0 0 0];                    % Eq19
C2_2  = [0 0 1 0];                    % Eq20

x0 = [0 0 0 0]';                      % original x

u = diff(x_c)/T;                    % uk is input on timestep k
u = [u, u(end)];                    % here u is a vector wrt time t

Q = 1e-8;                           % ** noise level in system dynamics

isUseCOP = 0;                       % 0 means not using COP, position only
R = eye(isUseCOP+1);                % ** noise level in measurement(table 2)
R(isUseCOP+1,isUseCOP+1) = 1e-5;
R(1,1) = 1e-5;
if isUseCOP
    y_S1 = [y1; y2]; %#ok<UNRCH>
    C2 = [C2_1;C2_2];
else
    y_S1 = y1;  
    C2 = C2_1;
end
[x_hat_array, ~, ~] = myEstimator(A2,B2,C2,u,y_S1,x0,Q,R);
x_force_estimated_S1       = x_hat_array(1,:);
dx_force_estimated_S1      = x_hat_array(2,:);
x_c_force_estimated_S1     = x_hat_array(3,:);
delta_x_force_estimated_S1 = x_hat_array(4,:);

isUseCOP = 1;                       % 1 means using COP,both COP and position
R = eye(isUseCOP+1);                % ** noise level in measurement(table 2)
R(isUseCOP+1,isUseCOP+1) = 1e-5;
R(1,1) = 1e-5;
if isUseCOP
    y_S1 = [y1; y2];
    C2 = [C2_1;C2_2];
else
    y_S1 = y1; %#ok<UNRCH>
    C2 = C2_1;
end
[x_hat_array, ~, ~] = myEstimator(A2,B2,C2,u,y_S1,x0,Q,R);
x_force_estimated_S12       = x_hat_array(1,:);
dx_force_estimated_S12      = x_hat_array(2,:);
x_c_force_estimated_S12     = x_hat_array(3,:);
delta_x_force_estimated_S12 = x_hat_array(4,:);
%% plot External force estimator
% t = 1:length(t);
figure('units','normalized','outerposition',[0 0 1 1],'name','Naive State estimation')
subplot(4,1,1)
plot(t, x,'b')
hold on
grid on
plot(t, x_force_estimated_S1,'r--','linewidth',2)
plot(t, x_force_estimated_S12,'k:','linewidth',2)
legend('Ground Truth Position', 'S1', 'S12')
title('Position')

subplot(4,1,2)
plot(t, dx,'b')
hold on
grid on
plot(t, dx_force_estimated_S1,'r--','linewidth',2)
plot(t, dx_force_estimated_S12,'k:','linewidth',2)
legend('Ground Truth velocity', 'S1','S12')
title('Velocity')

subplot(4,1,3)
plot(t, x_c,'b')
hold on
grid on
plot(t, x_c_force_estimated_S1,'r--','linewidth',2)
plot(t, x_c_force_estimated_S12,'k:','linewidth',2)
legend('Ground Truth COP', 'S1','S12')
title('COP')

subplot(4,1,4)
plot(t, u_x * ones(length(t)),'b')
hold on
grid on
plot(t, delta_x_force_estimated_S1,'r--','linewidth',2)
plot(t, delta_x_force_estimated_S12,'k:','linewidth',2)
legend('Ground Truth force', 'S1','S12')
title('force')