I am trying to write two filters to reduce noise in the data coming from a LIDAR

Question

I am trying to write two filters to reduce noise in the data coming from a LIDAR sensor attached to a robot using C++. The sensor can be accessed through a driver and each update of the sensor returns an array of float values of distance measurements of dimension N. N is typically in a range of ~[100, 800] measurements. Measured distances are typical in a range of [0.02 40] meters. A range filter The range filter crops all the values that are below range_min (resp. above range_max), and replace them with the range_min value (resp. range_max) A temporal median filter The temporal median filter calculates the median of several observations delayed up to D steps y_i(t) = median(x_i(t) + x_i(t- 1) + ... + x_i (t - D)) where x and y are input and output N-dim range vectors (i is in [0, N) interval). The number of delayed observations D is a parameter that should be given when creating a new temporal median filter.

Explanation / Answer

The Algorithmn for single Robot object tracking is :

Require: Input object list with Nin objects.

Require: Local object list with Nloc objects.

Ensure: Nin > 0 _ Nloc> 0.

1: for i 2 Lin do

2: for j 2 Lloc do

3:t = tin - tdet(j).

4: Temporal Kalman filter prediction with object j and t.

5: Possible pairs distance matrix PD[i; j] = DM(i; j).

6: end for

7: end for

8: Create selected pairs list.

9: while Size(Selected Pairs)< min(Nin; Nloc) do

10: Select minimum valid value of PD

11: if PD[i; j] < DMjmax then

12: Include pair (i; j) in selected pairs.

13: Mark row i and column j with invalid values.

14: else

15: Stop selecting pairs.

16: end if

17: end while

18: KF iteration for objects in selected pairs.

19: Eliminate objects from Lloc. . Not fulfilling elimination conditions

20: Add new objects not paired to Lloc.

21: Filter reduction to detect duplicates.

22: Update objects scoring.

LiDAR Sensor Data

sigma = 3;

halfwid = 5;

format long

%1D Mask

x = linspace(-halfwid, halfwid, 2*halfwid+1);

tmp1 = exp(-1./(sqrt(2.*sigma.^2))*(x.^2))

sum = 0;

for i=1:(2*halfwid+1)

sum = sum+tmp1(i);

end

tmp1 = tmp1./sum;

sprintf(’%0.10f & ’, tmp1)

%filename = uigetfile(’All Files (*.*)’)

fid = fopen(’20100802_171300_TrajLigneDroite3.dat’);

fName = ’output2.txt’;         %# A file name

fout = fopen(fName,’w’);

%Read Header File

F_ID = fread(fid, 8,’*char’) %First 8 Bytes

F_Options = fread(fid, 1) %Options - 1 Byte

F_Lookup_Table_Address = fread(fid, 1,’uint64’)

fseek(fid, 17, ’bof’);

%READ Time Stamp

%Read Network Packet

while true

timestamp = fread(fid, 1, ’int64’)

header_type = fread(fid, 1, ’int8’)

header_n = fread(fid, 1, ’int16’)

sensor_X = fread(fid, 1, ’float’)

sensor_Y = fread(fid, 1, ’float’)

sensor_Z = fread(fid, 1, ’float’)

sensor_Yaw = fread(fid, 1, ’float’)

sensor_Pitch = fread(fid, 1, ’float’)

sensor_Roll = fread(fid, 1, ’float’)

for i = 1:header_n

if ftell(fid) ~= F_Lookup_Table_Address

data_yaw(i) = fread(fid, 1, ’uint16’);

data_pitch(i) = fread(fid, 1, ’uint16’);

data_distance(i) = fread(fid, 1, ’uint16’);

data_intensity(i) = fread(fid, 1, ’uint8’);

fprintf(fout,’%d %d %d %d ’,data_yaw(i), data_pitch(i), data_distance(i), data_intensity(i));

else

break;

end

end

fprintf(fout,’ ’);

if ftell(fid) == F_Lookup_Table_Address

break;

end

end

fclose(fout);

fclose(fid);

Related Questions

Navigate

• Browse (All)
• Browse I
• Subjects

---

---

Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.