[ Prev ] [ Index ] [ Next ]

Numerical integration as an approach to derivation

Created Sonntag 01 Mai 2016

Concept


Numerical integration can be used as an approach to derivation.
Very often, in robotics appplication, we want to have velocity measurements of every robot joint. However, only position sensor is available. Thus, we decide to derive the joint position to get joint velocity.

However, the derivation porcess introduces high frequency noise on the velocity singal. Adding a low-pass filter can be a solution. Nevertheless, a better method is introduced. Using integration, it is actully possible to do differentitation with a help from a closed-loop control system..

./observer.odg

is any type of controller, the simplest one is a proportional (P) controller. The plant is an integrator. causes to track . Therefore, tracks .
If is the position signal, , will be the velocity signal.

Impementation and Results


MATLAB implementation: ./test01.m

function test01()

%% Generate noisy signal
Ts = 0.005;                                 % 200 Hz
t = 0 : Ts : 10;
y = 10 * sin(t);                            % Position signal
y_dot = 10 * cos(t);                        % Actual velocity

% Add some noises
A = 0.01;
noise = A * 2 * rand(1, size(t,2)) - 1;     % Evenly distributed noisy
y = y + noise;

% Compare with differentiation (Euler method)
for k = 1 : size(y,2) - 1
    v_dot_euler(k + 1) = (y(k + 1) - y(k)) / Ts;
end

%%
y_dot_est = pi_diff(y, Ts, 10, 0.5);

figure;
hold on
plot(t, v_dot_euler, '--g')
plot(t, y_dot_est, '--b')
plot(t, y_dot, 'r')

legend('With differentiation', 'With integration', 'True');

end

% With PI controller
function v_est = pi_diff(y, Ts, kp, ki)

error_acc = 0;
y_hat = 0;

for k = 1 : size(y,2)
    error = y(k) - y_hat;
    error_acc = error_acc + error * Ts;
    v_est(k) = kp * error + ki * error_acc;
    y_hat = y_hat  + v_est(k)* Ts;
end

end