The Object Tracking Algorithm

We iteratively keep tracking the positions and velocities of sensors by extrapolation based on the accelerations reported by g-sensors. However, accumulated error is a major con-cern and rotations of sensors can’t be detected by sensors themselves. So, we develop a heuristic by utilizing the relative positions between sensors to adjust the displacements and velocities of sensors by least square methods (abbreviated as LSM). In the mean while, rotations can also be revealed.

In addition to position Si,j and acceleration Ai,j, let Vi,j denote the velocity of the i-th sensor at time t_j measured in the earth frame, and eS_i,j+1 denote the estimated position of the i-th sensor at time tj+1 obtained from Si,j and Vi,j by extrapolation. Based on the topology information of the constellation, S_i,j+1 is obtained and V_i,j+1 is also calculated.

The process of object tracking from time tj to time tj+1 is depicted in Figure 5.5. The

S_1,j S

S

S

2,j 3,j

~

S~

S~

S_{2,j +1}

S_{1,j +1} S_{3,j +1}

LSMextrapolation ^{3,j +1}

2,j +1

1,j +1

Figure 5.5: Object tracking by extrapolation and least square method.

process will repeat again and again.

5.2.1 Extrapolation

Let 4t_j = t_j+1− t_j, i.e., the time span between the j-th and (j + 1)-th samples. Assume the motion from t_j to t_j+1 has a constant acceleration. Then, eS_i,j+1 can be obtained by applying the constant acceleration motion formula

Se_i,j+1= S_i,j+ V_i,j4t_j+ 1

2A_i,j(4t_j)². (5.3) This also is called the extrapolation method.

5.2.2 Least Square Method

Since the constellation is rigid, the topology metrics between sensors is fixed, especially the distance. However, these geometric properties may be broken by the extrapolation process. Therefore, the predicted positions can be adjusted based on topology information.

We apply the least square method (abbrev. as LSM) given in Alg. 1 to do adjustment. For convenience, if it is not necessary to emphasize the time index j, we frequently suppress the index j and simply use S_i to denote the position of the i-th sensor.

Let Si = (xi, yi, zi) for i = 1, 2, 3 be the position of the i-th sensor; and eSi = (exi, eyi, ezi) for i = 1, 2, 3 be the predicted position of the i-th sensor that is obtained by the extrapo-lation method. Due to the measurement error of acceleration and the accumuextrapo-lation error of velocity, the predicted position may not precise. Since the system is rigid, the topology metrics between S1, S2, S3 is fixed, especially the distance. We have kS2− S1k = l1,2, kS₃− S₂k = l_2,3, and kS₁− S₃k = l_1,3. So, we can calibrate the predicted position under the topology constraints by the least square method.

Algorithm 1 [Least Square Problem (LSP)]

for all eS_i, i = 1, 2, 3, and l_i,k, 1 ≤ i < k ≤ 3 such that kS_i− S_kk = l_i,k do

, for 1 ≤ i < k ≤ 3

Put all together and then we have

x₁+ x₂+ x₃ = x⁰₁+ x⁰₂+ x⁰₃,

Put a constellation at the barycenter of eS₁Se₂Se₃, then based on Theorem 3, the LSP can be solved by finding a best rotation. In this work, we further assume that S1S2S3 and Se₁Se₂Se₃ are in the same plane, and therefore, the solution can be found by a 2D rotation.

A numerical method is used to find the best rotation. The numerical method is used to solve the LSM which described by following :

In the beginning, whose vertices are S1, S2 and S3 and G is the center of mass of the triangle. Let ∠S₁GS₂ = θ₃, ∠S₂GS₃ = θ₁ and ∠S₃GS₁ = θ₂, then we can use law of cosines to get θ1, θ2 and θ3. After time 4t, the vertices of triangle become eS1, eS2 and Se₃, which are derived using the extrapolation method. So we can get eG the center of mass whose vertices are eS1, eS2 and eS3, which equals G by theorem 3. We also can decide the initial position of S₁. Assume the unit normal vector becomes u = (u_x, u_y, u_z) whose vertices are eS1, eS2 and eS3. Now, we need to find the coordinates of S2 and S3. Then find the best possible coordinates of S₁, S₂ and S₃. Then rotation about an arbitrary unit vector (ux, uy, uz) by an angle θ is given by a matrix by Theorem 1

Using the theorem above, we have −→

GS₂ = M(u, θ₃)−→

has the minimum. This means that

θ = arg minb

Having bθ, the best possible coordinates can be found.

5.2.3 Adjust Velocity

After applying the LSM, we get the positions S_i,j+1. We can further to adjust the velocities of sensors according to the displacement. Let Di,j = Si,j+1− Si,j be the displacement of the i-th sensor in the period from the j-th sampling time to the (j + 1)-the sampling time.

In addition, we also have

Di,j = Vi,j∆tj +1

2Ai,j(∆tj)²

= 1

2∆tj(Vi,j + Vi,j+1) . So, the velocity V_i,j+1 can be given by

V_i,j+1 = 2 (S_i,j+1− S_i,j)

∆tj

− V_i,j. (5.4)

5.2.4 Object Rotation

From Eq. 5.1 we can get the transformation matrix T_j^B→E and by Theorem 2 the Z−Y −X rotation matrix is known.

Finally the Figure 5.6 shows the procedure of this algorithm. In the beginning the coordinate, velocity and transformation matrix will be initiated. The alignment between g sensors will also be calculated in this section. After initial and alignment have done, the new coordinates of g sensors will be calculated by extrapolation. Then the corrected coordinates will be calculated from new coordinates by LSM. Finally the velocity and transformation matrix can be determined by initial coordinates and corrected coordinates.

Rotation information can also be got from the transformation matrix.

Initiate j = 0, Vi,0 = 0, Si,0, T , G

Calculate T and Ai,j by Eq. (1), (2)

Extrapolate Si,j+1

by Eq. (3)

Use LSM to obtain Si,j+1

from Si,j+1

Calculate velocity Vi,j+1

by Eq. (4)

j = j + 1

Si -> S1 B

i,0

jB -> E

~

~

(5.1), (5.2)

(5.3)

(5.4)

Figure 5.6: Flowchart of the object tracking algorithm.

Chapter 6 Experiments

In our experiments, the constellation is composed of three Witilt V3 sensors which has a triaxial accelerometer and a single axis gyroscope. But only the accelerometer is used.

The sampling rate is set up 90Hz. The sampling range is around ±6g. Three sensors are fixed on a plastic board at the vertices of an equilateral triangle. The edge of the equilateral triangle is 0.3m. The data of Witilt V3 sensors is output via a Bluetooth interface.