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

1 次查看（过去 30 天）
Junu Lee
Junu Lee 2021-10-28
评论： Junu Lee 2022-6-4
//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 个评论
Mathieu NOE
Mathieu NOE 2021-10-29
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)

请先登录，再进行评论。

请先登录，再回答此问题。

采纳的回答

Mathieu NOE
Mathieu NOE 2021-10-29
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

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Sensor Models 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by