My low pass filter is too much delayed than I wish.

3 visualizzazioni (ultimi 30 giorni)
//0.1Hz
#define A_COEFFICIENT 0.99373647154161460000
#define B_COEFFICIENT 0.00326293729757559350
float32* LPF_Velocity(float32 act_velo_x, float32 act_velo_y, float32 act_velo_z)
{
static float32 act_velo_filtered[3] = { 0.0, 0.0, 0.0 };
static float32 act_velo_y1[3] = { 0.0, 0.0, 0.0 };
static float32 act_velo_x1[3] = { 0.0, 0.0, 0.0 };
//Filtering (Sampling Frequency @ 100 Hz, Cutoff Frequency @ 1Hz)
act_velo_filtered[0] = A_COEFFICIENT * act_velo_y1[0] + B_COEFFICIENT * act_velo_x + B_COEFFICIENT * act_velo_x1[0];
act_velo_y1[0] = act_velo_filtered[0];
act_velo_x1[0] = act_velo_x;
act_velo_filtered[1] = A_COEFFICIENT * act_velo_y1[1] + B_COEFFICIENT * act_velo_y + B_COEFFICIENT * act_velo_x1[1];
act_velo_y1[1] = act_velo_filtered[1];
act_velo_x1[1] = act_velo_y;
act_velo_filtered[2] = A_COEFFICIENT * act_velo_y1[2] + B_COEFFICIENT * act_velo_z + B_COEFFICIENT * act_velo_x1[2];
act_velo_y1[2] = act_velo_filtered[2];
act_velo_x1[2] = act_velo_z;
return act_velo_filtered;
}
float32* LPF_Acceleration(float32 act_acc_x, float32 act_acc_y, float32 act_acc_z)
{
static float32 act_acc_filtered[3] = { 0.0 , 0.0, 0.0 };
static float32 act_acc_y1[3] = { 0.0 , 0.0, 0.0 };
static float32 act_acc_x1[3] = { 0.0 , 0.0, 0.0 };
//Filtering (Sampling Frequency @ 100 Hz, Cutoff Frequency @ 1Hz)
act_acc_filtered[0] = A_COEFFICIENT * act_acc_y1[0] + B_COEFFICIENT * act_acc_x + B_COEFFICIENT * act_acc_x1[0];
act_acc_y1[0] = act_acc_filtered[0];
act_acc_x1[0] = act_acc_x;
act_acc_filtered[1] = A_COEFFICIENT * act_acc_y1[1] + B_COEFFICIENT * act_acc_y + B_COEFFICIENT * act_acc_x1[1];
act_acc_y1[1] = act_acc_filtered[1];
act_acc_x1[1] = act_acc_y;
act_acc_filtered[2] = A_COEFFICIENT * act_acc_y1[2] + B_COEFFICIENT * act_acc_z + B_COEFFICIENT * act_acc_x1[2];
act_acc_y1[2] = act_acc_filtered[2];
act_acc_x1[2] = act_acc_z;
return act_acc_filtered;
}
too much delayed...
Above code is low pass fitler what I made in Code Composer Studio.
I am using TMS320F28335 to operate robot arm.
There are three encoder. I obtain the signal of angle each joints.
And then I take the angular velocity and the angular acceleration by the first derivative.
Of cource, the first derivative make many noises on the signal.
So this is why I apply the low pass filter to them each joints.
Control frequency is 100Hz. Cut-off frequency is 0.1Hz becuase period of the trajectory of the robot arm is 10s.
But the result is unexpectable.
As far as I know, the first order of low pass filter has a delay only one sampling time(0.01s on my system)
But the result has too much delay than I know.
I calculated the transfer function of the first order of low pass filter and then I converted the transfer function to z-domain.
And then convert the z-plane to discrete equation as following equation.
y[n] = a*y[n-1] + b*x[n] + b*x[n-1]
And then I just make the code as like above one.
I don't know why there is much delayed on my signal.
Please help me.
I uploaded my matlab code for plotting the signal and data of result from operating system.
If the cut-off frequency is too low, then does butterworth filter make distortion of the signal?
  1 Commento
Mathieu NOE
Mathieu NOE il 29 Ott 2021
hello
the delay of a filter depends of it's type (bessel, butter, elliptic,..) , order and cut off frequency (fc)
if you increase the order (at fixed fc) or lower fc at fixed order , the delay will increase
there is one sample delay as you go from analog to digital and back to analog domains (so ADC + DAC cost you 1 additionnal sample delay) but the filter will cost you much more.
Simply compute the transfer function of your filter and look at the group delay (or impulse response)

Accedi per commentare.

Risposta accettata

Mathieu NOE
Mathieu NOE il 29 Ott 2021
hello again a simple test on Joint4 data with a butterworth filter order 2 and fc = 1 Hz
the first and second derivatives are obtained by central difference derivative (see details in code)
%% 1. Joint Space TDC(Trajectory - Position - Angle - Angle Velocity - Angle Acceleration)
clc; clear; close all;
% Actual Joint Data
Act_Joint_Data = load('10s_Actual_LPF(0.1Hz).txt');
% 데이터 취득
ActualRth1 = Act_Joint_Data(:, 1)/10000;
ActualRth2 = Act_Joint_Data(:, 2)/10000;
ActualRth4 = Act_Joint_Data(:, 3)/10000;
ActualAngVelo1 = Act_Joint_Data(:, 4)/100000;
ActualAngVelo2 = Act_Joint_Data(:, 5)/100000;
ActualAngVelo4 = Act_Joint_Data(:, 6)/100000;
ActualAngAcc1 = Act_Joint_Data(:, 7)/100000;
ActualAngAcc2 = Act_Joint_Data(:, 8)/100000;
ActualAngAcc4 = Act_Joint_Data(:, 9)/100000;
Time = Act_Joint_Data(:, 10)/100;
% Desired Data
Des_Data = load('10s_Desired_LPF.txt');
% 데이터 취득
DesiredRth1 = Des_Data(1:length(Time), 1)/10000;
DesiredRth2 = Des_Data(1:length(Time), 2)/10000;
DesiredRth4 = Des_Data(1:length(Time), 3)/10000;
DesiredAngVelo1 = Des_Data(1:length(Time), 4)/100000;
DesiredAngVelo2 = Des_Data(1:length(Time), 5)/100000;
DesiredAngVelo4 = Des_Data(1:length(Time), 6)/100000;
DesiredAngAcc1 = Des_Data(1:length(Time), 7)/100000;
DesiredAngAcc2 = Des_Data(1:length(Time), 8)/100000;
DesiredAngAcc4 = Des_Data(1:length(Time), 9)/100000;
% 상수 값
Period = 5;
Frequency = 1/Period;
AngFreq = 2*pi*Frequency;
Rd1 = 0.264; Ra2 = 0.420; Ra3 = 0.4860;
XAmp = 0.1; YAmp = -0.0707; ZAmp = 0.0707;
% Actual 순기구학
ActualX = Rd1 - Ra2.*sin(ActualRth2) - Ra3.*cos(ActualRth4).*sin(ActualRth2);
ActualY = Ra2.*cos(ActualRth1).*cos(ActualRth2) - Ra3.*sin(ActualRth1).*sin(ActualRth4) + Ra3.*cos(ActualRth1).*cos(ActualRth2).*cos(ActualRth4);
ActualZ = Ra2.*cos(ActualRth2).*sin(ActualRth1) + Ra3.*cos(ActualRth1).*sin(ActualRth4) + Ra3.*cos(ActualRth2).*cos(ActualRth4).*sin(ActualRth1);
% Desired 순기구학
DesiredX = Rd1 - Ra2.*sin(DesiredRth2) - Ra3.*cos(DesiredRth4).*sin(DesiredRth2);
DesiredY = Ra2.*cos(DesiredRth1).*cos(DesiredRth2) - Ra3.*sin(DesiredRth1).*sin(DesiredRth4) + Ra3.*cos(DesiredRth1).*cos(DesiredRth2).*cos(DesiredRth4);
DesiredZ = Ra2.*cos(DesiredRth2).*sin(DesiredRth1) + Ra3.*cos(DesiredRth1).*sin(DesiredRth4) + Ra3.*cos(DesiredRth2).*cos(DesiredRth4).*sin(DesiredRth1);
% 오차 계산
ErrorSum = 0;
ErrorX = DesiredX - ActualX;
ErrorY = DesiredY - ActualY;
ErrorZ = DesiredZ - ActualZ;
% Filter_err = 0;
for index = 1 : 1 : length(Act_Joint_Data)
ErrorSum = ErrorSum + ErrorX(index, 1)^2 + ErrorY(index, 1)^2 + ErrorZ(index, 1)^2;
% Filter_err = Filter_err + ActualAngVelo1_MATLPF(index, 1) - ActualAngAcc1(index, 1);
end
RMS = sqrt(ErrorSum)
% Plot
% Fig. 1. Trajectory
figure(1)
plot3(DesiredX, DesiredY, DesiredZ, ActualX, ActualY, ActualZ)
hold on
plot3(DesiredX(1, 1), DesiredY(1, 1), DesiredZ(1, 1), 'mo', ActualX(1, 1), ActualY(1, 1), ActualZ(1, 1))
grid on
title('Trajectory', 'FontSize', 15)
legend('Desired', 'Actual')
xlabel('x(m)', 'FontSize', 12)
ylabel('y(m)', 'FontSize', 12)
zlabel('z(m)', 'FontSize', 12)
% Fig. 2. Error
figure(2)
subplot(3, 1, 1)
plot(Time, zeros(length(Time), 1), Time, ErrorX)
grid on
title('Error of Position X')
xlabel('time(s)', 'FontSize', 11)
ylabel('x(m)', 'FontSize', 11)
legend('Desired', 'Error of X')
subplot(3, 1, 2)
plot(Time, zeros(length(Time), 1), Time, ErrorY)
grid on
title('Error of Position Y')
xlabel('time(s)', 'FontSize', 11)
ylabel('x(m)', 'FontSize', 11)
legend('Desired', 'Error of Y')
subplot(3, 1, 3)
plot(Time, zeros(length(Time), 1), Time, ErrorZ)
grid on
title('Error of Position Z')
xlabel('time(s)', 'FontSize', 11)
ylabel('x(m)', 'FontSize', 11)
legend('Desired', 'Error of Z')
% Fig. 4. Joint Space
figure(3)
% Angle
subplot(3, 3, 1)
plot(Time, DesiredRth1, Time, ActualRth1)
grid on
title('Joint1', 'FontSize', 12)
xlabel('time(s)', 'FontSize', 11)
ylabel('theta(rad)', 'FontSize', 11)
legend('Desired', 'Actual')
subplot(3, 3, 2)
plot(Time, DesiredRth2, Time, ActualRth2)
grid on
title('Joint2', 'FontSize', 12)
xlabel('time(s)', 'FontSize', 11)
ylabel('theta(rad)', 'FontSize', 11)
legend('Desired', 'Actual')
subplot(3, 3, 3)
plot(Time, DesiredRth4, Time, ActualRth4)
grid on
title('Joint4', 'FontSize', 12)
xlabel('time(s)', 'FontSize', 11)
ylabel('theta(rad)', 'FontSize', 11)
legend('Desired', 'Actual')
% do my own first and second derivatives on Joint4 (ActualRth4)
[dActualRth4, ddActualRth4] = firstsecondderivatives(Time,ActualRth4);
% add some low pass filter
fc = 2;
Fs = 100;
[b,a] = butter(2,2*fc/Fs);
dActualRth4 = filter(b,a,dActualRth4);
ddActualRth4 = filter(b,a,ddActualRth4);
% Joint Velocity
subplot(3, 3, 4)
plot(Time, DesiredAngVelo1, Time, ActualAngVelo1)
grid on
title('Joint1 Angular Velocity')
xlabel('time(s)', 'FontSize', 11)
ylabel('angular velocity(rad/s)', 'FontSize', 11)
legend('Desired', 'Actual')
subplot(3, 3, 5)
plot(Time, DesiredAngVelo2, Time, ActualAngVelo2)
grid on
title('Joint2 Angular Velocity')
xlabel('time(s)', 'FontSize', 11)
ylabel('angular velocity(rad/s', 'FontSize', 11)
legend('Desired', 'Actual')
subplot(3, 3, 6)
plot(Time, DesiredAngVelo4, Time, ActualAngVelo4, Time, dActualRth4)
grid on
title('Joint4 Angular Velocity')
xlabel('time(s)', 'FontSize', 11)
ylabel('angular velocity(rad/s', 'FontSize', 11)
legend('Desired', 'Actual', 'filtered')
% Joint Acceleration
subplot(3, 3, 7)
plot(Time, DesiredAngAcc1, Time, ActualAngAcc1)
grid on
title('Joint1 Angular Acceleration')
xlabel('time(s)', 'FontSize', 11)
ylabel('angular acceleration(rad/s^2)', 'FontSize', 11)
legend('Desired', 'Actual')
subplot(3, 3, 8)
plot(Time, DesiredAngAcc2, Time, ActualAngAcc2)
grid on
title('Joint2 Angular Acceleration')
xlabel('time(s)', 'FontSize', 11)
ylabel('angular acceleration(rad/s^2)', 'FontSize', 11)
legend('Desired', 'Actual')
subplot(3, 3, 9)
plot(Time, DesiredAngAcc4, Time, ActualAngAcc4, Time, ddActualRth4)
grid on
title('Joint4 Angular Acceleration')
xlabel('time(s)', 'FontSize', 11)
ylabel('angular acceleration(rad/s^2)', 'FontSize', 11)
legend('Desired', 'Actual', 'filtered')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dy, ddy] = firstsecondderivatives(x,y)
% The function calculates the first & second derivative of a function that is given by a set
% of points. The first derivatives at the first and last points are calculated by
% the 3 point forward and 3 point backward finite difference scheme respectively.
% The first derivatives at all the other points are calculated by the 2 point
% central approach.
% The second derivatives at the first and last points are calculated by
% the 4 point forward and 4 point backward finite difference scheme respectively.
% The second derivatives at all the other points are calculated by the 3 point
% central approach.
n = length (x);
dy = zeros;
ddy = zeros;
% Input variables:
% x: vector with the x the data points.
% y: vector with the f(x) data points.
% Output variable:
% dy: Vector with first derivative at each point.
% ddy: Vector with second derivative at each point.
dy(1) = (-3*y(1) + 4*y(2) - y(3)) / (2*(x(2) - x(1))); % First derivative
ddy(1) = (2*y(1) - 5*y(2) + 4*y(3) - y(4)) / (x(2) - x(1))^2; % Second derivative
for i = 2:n-1
dy(i) = (y(i+1) - y(i-1)) / (x(i+1) - x(i-1));
ddy(i) = (y(i-1) - 2*y(i) + y(i+1)) / (x(i-1) - x(i))^2;
end
dy(n) = (y(n-2) - 4*y(n-1) + 3*y(n)) / (2*(x(n) - x(n-1)));
ddy(n) = (-y(n-3) + 4*y(n-2) - 5*y(n-1) + 2*y(n)) / (x(n) - x(n-1))^2;
end
  3 Commenti
Mathieu NOE
Mathieu NOE il 19 Nov 2021
hello
how is it going ?
problem solved ?
all the best
Junu Lee
Junu Lee il 4 Giu 2022
Sorry for late reply, Now I am trying to apply your code,,,, Sorry for late...
I'll reply more.

Accedi per commentare.

Più risposte (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by