I am novice in Slam. I want to implement EKF based slam. For that I consider the book
PROBABILISTIC ROBOTICS by
Sebastian THRUN. The algorithm they derive in this book for
EKF SLAM FOR KNOWN CORRESPONDENCE. I have some doubt with this algorithm.
Hope it will be clear after this post.
I post a picture to show my doubts in this algorithm. Blue and red box arise doubts. Unable to understand what did it mean.
In this algorithm line no: 6 they calculate $Q_t$. As per my knowledge $r,p_i$ they are range and bearing measurement. I have a data file which consist of 88000 value for range and bearing for different time step. So can I consider all this value to calculate $Q_t$ matrix?
In this algorithm can anyone explain me line no 9? $\mu(j,x), \mu(j,y), \mu(j,s)$ are they Cartesian co-ordinate of landmarks? $\mu(t,x), \mu(t,y)$ are they Cartesian co-ordinate of Robot Pose? If so, there is no sign of timestamp in $\mu(j,x), \mu(j,y)$ but there is a $t$ associate with $\mu(t,x), \mu(t,y)$. which $t$ it is?
There is written that if landmark $j$ never seen before then only calculate line no: 9.
My range and bearing data file are look like
# Time [s] Subject # range [m] bearing [rad] 1248272276.038 90 2.148 0.025 1248272276.727 14 3.387 0.006 1248272277.773 14 3.309 -0.015 1248272277.773 90 2.033 0.024 1248272278.237 14 3.272 -0.023 1248272278.969 90 1.946 0.040 1248272279.453 14 3.165 -0.051 1248272280.199 14 3.118 -0.070
So as per my data file same landmark repeated several time dependent upon Robot movement. How could I manipulate them as per line No. 10
My odometry data file look like that
# Time [s] forward velocity [m/s] angular velocity[rad/s] 1248272272.841 0.074 0.229 1248272272.852 0.074 0.229 1248272272.903 0.074 0.229 1248272272.964 0.074 0.229 1248272272.995 0.074 0.229 1248272273.005 0.074 0.230
So the odometry data's timestamp are not matched with range bearing timestamp. They are different. So How could I calculate? Because in line no 3 of this algorithm I am using odometer timestamp but in line no. 9 I am using range bearing time stamp but they are different. Then How could I calculate mu(j,x),mu(j,y) from that?