Full-state State Observer

main.m

close all

% SIMULATION

m = 1;
I = 0.1677;
R = 0.02;
h = 0.02;
g = 9.81;

A = [0, 0, 0, -m*g/I; 1, 0, 0, 0; 0, -2/3*g, 0, 0; 0, 0, 1, 0];
B = [1, 0, 0, 0]';
C = [0, 1, 0, 0];
D = 0;

RealSystem = ss(A, B, C, D);
X0 = [0, 0.1, 0, 0.035]';
tmax = 1;
[y_real,t_real,x_real] = initial(RealSystem, X0, tmax);

fig = figure();
subplot(2,1,1);
plot(t_real, y_real)
ylabel('theta (rad)')
xlabel('t (s)')
subplot(2,1,2);
plot(t_real, x_real(:,4))
ylabel('r (m)')
xlabel('t (s)')
saveas(fig, 'initial.png')

% OBSERVER

Q = 3e4; R = 0.01;
[kest,L,P] = kalman(RealSystem,Q,R,[]);
disp(L)
eig(A-L*C)

xdot = @(t, x) A*x + L * (interp1(t_real,y_real,t) - C*x);
[t_observed, x_observed] = ode45(xdot, [0,tmax], [0,0,0,0]);

fig = figure();
plot(t_real, x_real);
hold on;
set(gca,'ColorOrderIndex',1)
plot(t_observed, x_observed, '--');
ylabel('X')
xlabel('t')
ylim([-.5, 1])
legend('theta-dot', 'theta', 'r-dot', 'r', 'theta-dot (observed)', 'theta (observed)', 'r-dot (observed)', 'r (observed)')
saveas(fig, 'observed.png')

% ANIMATION 1

for t=0:0.01:tmax
    f = figure('visible','off');
    X = interp1(t_real,x_real,t);
    theta = X(2);
    r = X(4);
    plot([-0.1*cos(theta), 0.1*cos(theta)], [-0.1*sin(theta), 0.1*sin(theta)], 'LineWidth', 2);
    hold on
    viscircles([r*cos(theta)+R*sin(theta) r*sin(theta)+R*cos(theta)], R);
    xlim([-0.1,0.1]);
    ylim([-0.1,0.1]);
    axis('equal');
    saveas(f, sprintf('sim-%.2f.png', t));
end

% ANIMATION 2

for t=0:0.01:tmax
    f = figure('visible','off');
    X = interp1(t_real,x_real,t);
    X_observed = interp1(t_observed,x_observed,t);
    theta = X(2);
    r = X(4);
    theta_obs = X_observed(2);
    r_obs = X_observed(4);
    plot([-0.1*cos(theta), 0.1*cos(theta)], [-0.1*sin(theta), 0.1*sin(theta)], 'LineWidth', 2)
    hold on
    plot([-0.1*cos(theta_obs), 0.1*cos(theta_obs)], [-0.1*sin(theta_obs), 0.1*sin(theta_obs)], '--', 'LineWidth', 2)
    hold on
    viscircles([r*cos(theta)+R*sin(theta) r*sin(theta)+R*cos(theta)], [R])
    hold on
    viscircles([r_obs*cos(theta_obs)+R*sin(theta_obs) r_obs*sin(theta_obs)+R*cos(theta_obs)], [R], 'LineStyle', '--')
    xlim([-0.1,0.1])
    ylim([-0.1,0.1])
    axis('equal')
    saveas(f, sprintf('obs-%.2f.png', t));
end