Velocity Units

[THAC00]

Hey Praveen,

I've tried to access the Cluster Values via KF0.statePost.at(0), 1, 2, 3.. etc. However the Velocity values (2) and (3) for vx and vy are numbers of the order 10^(-6)

Do you remember what Unit that was or how I could convert those to m/s. Are those angles possibly? Best regards

[praveen-palanisamy]

Hi again @THAC00 ,

The units of elements in KF0.statePost (and K0.statePre) are based on the Kalman Filter state definition and initialization and the units of measurements. In the code, the State is defined to be [x, y, v_x, v_y] to denote the distance and velocity components along x and y respectively and the measurements are x and y location components of the point cloud object clusters. The units of those state variables depends on the units of the initial state and the units of the measurements. I used meters for the measurements. So the velocity vector will be in m/s. If you provide the inputs in meters as well then the velocity will be in m/s.

Your observation of v_x and v_y with very low values (order of 10^-6) may be because the KF has not had enough measurements or update steps after the initial state value (of 0 as initialized in the code in the following lines: )https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/blob/8bd23a2ee7325f449bbd746589348c90ffb4de7d/src/main.cpp#L474-L475

Or may be the actual velocity of the tracked point cloud object cluster is very low (close to zero).

[THAC00]

thanks for the reply!

I'm tracking objects such as people and vehicles which are moving at roughly 5km/h ( or 1.4 m/s) and this code tracks them over a path of 15 meters for almost the entire duration, so I'm a bit confused as to why it's so low, because it's getting a lot of measurements to update the predictions. (clusters are consistently recognized throughout the frames)

The input is a classical pcl sensor msgs, provided in meters, frame rate is 30 fp/s and sensor is working at 10Hz.

I'll try to initialize the velocities differently, maybe this helps!

If you don't have any other suggestion I still thank you for the time and effort you put in to reply!

[THAC00]

I've tried initializing with values of 1 m/s but it doesn't make a difference, very odd

This would be an excerpt of rostopic echo of one segmented cluster:

height: 1
width: 315
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 16
row_step: 5040
data: [104, 172, 112, 65, 245, 42, 2, 192, 24, 145, 190, 61, 0, 0, 128, 63, 179, 120, 119, 65, 188, 30, 134, 191, 184, 211, 199, 61, 0, 0, 128, 63, 9, 130, 121, 65, 184, 24, 18, 192, 212, 160, 212, 61, 0, 0, 128, 63, 82, 197, 113, 65, 54, 181, 2, 192, 240, 134, 229, 61, 0, 0, 128, 63, 24, 149, 114, 65, 250, 130, 91, 191, 54, 231, 240, 61, 0, 0, 128, 63, 76, 2, 113, 65, 252, 53, 2, 192, 134, 74, 54, 62, 0, 0, 128, 63, 230, 101, 117, 65, 239, 192, 242, 191, 194, 205, 41, 62, 0, 0, 128, 63, 190, 170, 117, 65, 223, 20, 133, 191, 78, 166, 37, 62, 0, 0, 128, 63, 16, 216, 112, 65, 63, 209, 89, 191, 74, 80, 75, 62, 0, 0, 128, 63, 88, 232, 111, 65, 112, 149, 215, 191, 186, 158, 126, 62, 0, 0, 128, 63, 20, 190, 112, 65, 17, 108, 216, 191, 178, 229, 76, 62, 0, 0, 128, 63, 202, 26, 112, 65, 224, 223, 194, 191, 170, 55, 105, 62, 0, 0, 128, 63, 65, 89, 112, 65, 225, 105, 173, 191, 222, 109, 101, 62, 0, 0, 128, 63, 72, 161, 112, 65, 207, 240, 151, 191, 236, 117

[praveen-palanisamy]

Oh. If you observe such a behavior when you are tracking objects for several meters, I can see two things that can improve the performance:

• Tuning the process noise co-variance matrix

  Because we are using a constant velocity model and using the position estimates as measurements, setting a lower value for the diagonal elements of the process noise co-variance matrix corresponding to the position estimates and a higher uncertainty value for the velocity state estimates will be better. Essentially, you can try changing the sigmaP values and consider using different values for the position and velocity uncertainties. Note that the values in the process noise co-variance matrix has units! The sigma values for the position estimates are in m^2 units (if we are using positions in meters) and the sigma values for the velocity estimates are in m^2/s^2 units. The corresponding lines to look at for this tuning is: https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/blob/8bd23a2ee7325f449bbd746589348c90ffb4de7d/src/main.cpp#L375-L377

• Using a better state transition model You could also use a better process/transition model than the constant velocity model. For example, you could use a 6 dimensional state vector that includes acceleration so that your new state vector looks like this: [x, y, v_x, v_y, a_x, a_y] and you can set the transition model to be like this:

  KF0.transitionMatrix = *(Mat_(6, 6) << 1,0,1,0,0.5,0,  0,1,0,1,0,0.5,  0,0,1,0,1,0,  0,0,0,1,0,1,  0,0,0,0,1,0,  0,0,0,0,0,1);

[THAC00]

Thanks a lot, I will try both and close the topic for now , since the original question was answered.

Best wishes

[THAC00]

Just to inform you about the suggestions: I've played around with all the values and they only make the values more unstable!

The order of magnitude stays the same. Now what I tried is outputting the velocities * 10^6, meaning for example:

1000000 KF5.statePost.at(2) and 1000000 KF5.statePost.at(3)

For a sequence of someone walking through the scene and getting tracked throughout, it outputs something around 1.5 m/s which is walking speed.

It may be a coincidence, however I feel like somewhere, something is simply altering the statePost output by 10^(-6).

Thanks again!