Created Sonntag 01 Mai 2016
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..
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