The reconstruction of a slice of a log has a shape of an ellipse. Clearly, from that shape it is possible to deduce the position of the body. When the log is not located in the center of the conveyor, it will reect in the reconstruction. In addition, all rotations of the log are depicted as changing the half-lengths of the ellipse on the reconstruction image. Therefore, we decided to utilize the reconstruction of the slice
of the timber in order to determine the position of the log. Hence, the next problem is to calculate the equation of this ellipse. First, we consider the analytical equation of the ellipse and further we describe how the equation can be calculated numerically from obtained reconstruction.
Our mathematical model assumes that X-rays are cast in straight lines. There-fore, we can represent X-rays as a plane which intersects the moving log. On the other hand, the log tends to have a shape of a cylinder. So we decided to represent the log as the cylinder with some radius and length. Next, we derive the equation of intersection of a cylinder and a plane. One can proof that if cylinder axis is not parallel to the plane, then the intersection is an ellipse (Figure 9).
Figure 9: Intersection of a cylinder and a plane.
A plane in three-dimensional space can be determined by a pointU on the plane and a unit-length normal vectorN. A pointXis on the plane wheneverNτ(X−U) = 0is satised. It states that normal vector is orthogonal to vector(X−U). Alternatively, a plane may be parameterized as a set of all points of the form:
X(α, β) = U +αA+βB, (16)
whereAandB are given linearly independent unit-length vectors dening the plane which are perpendicular to N. The parameters α and β are any real number. For simplicity, we can assume that N, A, and B are mutually perpendicular.
In calculation, we are going to assume that the cylinder is innite. In other words, the length of cylinder h equals to innity. We are allowed to assume that because
5 OBSERVATION MODEL 30 we can check the presence of intersection between the log and X-rays beforehand and only after that calculate the equation of intersection if it is required. An in-nite cylinder can be determined by point C which is located in the middle of it, by unit-length direction Dand its radius. Next we derive the equation that represents the cylinder.
The cylinder consists of points which lie at distance r from cylinder axis. Let us formulate this in mathematical language. We say a point X belongs to cylinder if the length of the projection of vector (X −C) to the plane perpendicular to D equals to r. So the rst step is to compute the projection of some vector V to the plane perpendicular toD(further we will call itF), which contains the origin. From geometry we know that V =V k F +V ⊥ F where V k F is the projection of V onto plane F and V ⊥F is the projection of V ontoD(normal vector of F). Thus, V kF =V −V ⊥F. According to [18]
V ⊥F = DτV D
|D|2 .
Previously, we suggest that D is unit-length, hence V ⊥F =DDτV. So we can rewrite
V kF =V −V ⊥F =V −DDτV = (I −DDτ)V.
Here I is the identity matrix. The matrix G = (I −DDτ) is the called projection matrix. The vector V0 =GV is in the plane with normal vector D and origin. The projection matrix has few properties. The main one is G2 = G. It means that projection of already projected vector is the same vector. What is more, the matrix G is symmetric e.t. Gτ = G. As we mentioned earlier, we have to project vector (X −C) to the plane perpendicular to D and its length should be equal to r, in which case|G(X−C)|=r. Then
r2 =|G(X−C)|2 = (G(X−C))τ(G(X−C)) =
= (X−C)τGτG(X−C) = (X−C)τGG(X−C) =
= (X−C)τG(X−C) = (X−C)τ(I−DDτ)(X−C). (17) Thus, the algebraic equation is the quadratic equation(X−C)τ(I−DDτ)(X−C) = r2. Next, we calculate the equation of intersection which is said to be an ellipse. We substitute (20) in (17). DeningE =U −C leads to (E+αA+βB)τG(E+αA+ βB) = r2. Expanding this, we obtain the quadratic equation for α and β,
AτGAα2+ 2AτGBαβ+BτGBβ2+ 2AτGEα+ 2BτGEβ+ (EτGE−r2) = 0.
The general quadratic equation for ellipse is
a11α2 +a12αβ+a22β2+a1α+a2β+a0 = 0. (18)
In addition, we can use the property of ellipse thata11a22−a4212 >0. Hence,a11>0 and a22>0. Next we assume that a12= 0. It is equivalent to the assumption that N kD. We have made that assumption because in our simulations a12 was always near zero. Thereby, we assume that A, B and D are mutually perpendicular, then
a12= 2(AτGB) = 2(Aτ(I−DDτ)B) = 2(AτB−(AτD)(DτB)) = 0.
If a12= 0, then we can transform equation (18) into the form:
(α−α0
Actually, we are not interested in the axis half-lengths of ellipse because dierent logs could have dierent radiuses. Therefore, we are more interested in ratio between them u=a22/a11 ∝p
a22/a11 =Lα/Lβ. Hence, our observation is as follows:
~
These three numbers are our observation. We can calculate them analytically by using equations which were mentioned earlier. Clearly, the observation operator is nonlinear. Therefore, it is necessary to calculate the Jacobian of the observation operator:
5 OBSERVATION MODEL 32
In section 4.4, we decided that state vector will include coordinates of the center of cylinder and quaternion, which is used to represent a rotation of the log. State vector has the form (4). In addition, a quaternion can be easily translated into left-handed rotation matrix P:
P=
Initially, the log has directionD=h
0 0 1 iτ
. At timet we can calculate direction of the cylinder as following:
Dt=P D =
As we said, X-rays can be represented as a plane. This plane has normal vectorN = h
. Therefore, we can denote A = h
. Clearly, N, A and B are mutually perpendicular. Also assume that position of the center of cylinder at initial time step is located at origin, then displacement is equal to position of logC =h
c1 c2 c3 iτ
. Now we can estimate the coecients of the equation of ellipse as follows:
a11=1−(2wy+ 2xz)2,
The Jacobian of observation operator now can be calculated by using the formulas above.
Further, we are going to illustrate how the presence of intersection can be dened.
As was mentioned earlier, a plane in three-dimensional space can be determined by point U and a unit-length normal vector N. A point X is on the plane whenever Nτ(X−U) = 0. In our simulation sensors' plane intersect point U = h
. We can dene cylinder in parametric form as cylinder. Then the position of point x at some time step equals P v+D, where P is a rotation and D is a displacement. By substituting it to the equation of a plane we get:
Nτ(X−U) = 0⇒Nτ(P v+D−U) = 0. (20) By substituting values forU,D and P into (20) we get:
P31x+P32y+P33z+D3−h1 = 0⇒z = (h1−D3−P31x−P32y)/P33. (21) Next, we substitute (19) into (21):
z = (h1−D3−P31rcos(t)−P32rsin(t))/P33,
as we said z belongs to [−h, h]. Thus, if there is some z belongs to [−h, h], then plain intersect the cylinder.
Finally, the process of parameters estimation should be illustrated. The second-order curve has the degree of freedom of 5, so ellipse can be dened by any ve arbitrary points on the plane. As long as no three of the ve points are coplanar, there exists a unique second-order curve that passes exactly through each of the ve points. One possible choice for tting an ellipse is to employ the general quadratic form of a second-order curve as given in equation (18). Let us assume that a0 is not equal to zero, then we arbitrary assign a0 = 1. This gives
a11α2+a12αβ+a22β2+a1α+a2β+ 1 = 0.
5 OBSERVATION MODEL 34 By selecting random points (αi, βi) on the edge of ellipse we induce a system of linear equations, where a11, a12, a22, a1, a2 are unknowns:
a11α2i +a12αiβi+a22βi2+a1αi+a2βi =−1.
In addition, we previously assumed that a12 is equal to 0, then we derive
a11α2i +a22βi2 +a1αi+a2βi =−1. (22) From the system of linear equations (22) it is clear that it is enough to select at least 4 points on the edge of a ellipse in order to solve the system. Unfortunately, recon-struction of the slice of a log is subject to computation error. In addition, points can be selected inappropriately so that the desired ellipse is not covered completely.
To overcome this problem, the number of selected points can be increased, thereby the system (22) will be overdetermined. Then, we have to employ least squares method, which is used for solving overdetermined systems. In addition, we can per-form tting more than one time in order to decrease noise in estimated parameters.
Therefore, we perform a series of tests for discovering an optimal number of points and the amount of tting that have to be executed for minimizing estimation error in case of reconstruction with resolution 1000×1000 .
number of tting repetitions
10 100 500 1000
numberofpoints 4 48.3899 23.3096 37.6625 23.3362 10 0.0336 0.0131 0.0061 0.0046 20 0.0170 0.0055 0.0025 0.0019 40 0.0113 0.0038 0.0020 0.0015 60 0.0094 0.0031 0.0017 0.0014 80 0.0083 0.0027 0.0016 0.0014 100 0.0071 0.0025 0.0015 0.0014
Table 3: Mean value of estimation error of ellipse tting
According to the tables 3 and 4, the best approximation is achieved for 1000 tting repetitions with selecting 100 random points. It is worth mentioning that this oper-ation takes 108 ms for estimating one set of parameters. Therefore, we used current approach but with 500 repetitions and 100 random points. This takes 50ms, that is satisfactory for our purposes.
number of tting repetitions
10 100 500 1000
numberofpoints 4 818.1946 117.9103 396.8588 101.5653 10 0.0277 0.0141 0.0040 0.0033 20 0.0093 0.0029 0.0013 0.0010 40 0.0058 0.0020 0.0010 0.0008 60 0.0049 0.0016 0.0008 0.0006 80 0.0041 0.0014 0.0007 0.0006 100 0.0037 0.0013 0.0007 0.0005
Table 4: Standard deviation of estimation error of ellipse tting
6 Implementation
6.1 General overview
In this section, we provide detailed clarications on the numerical experiments in-tended to analyze the applicability of the exin-tended Kalman lter for the problem of estimating the position of the log on the moving conveyor belt. The description of the extended Kalman lter was given in Section 3.2. The numerical experiments covered in this section use measurements obtained from a simulated motion of the log with the set of parameters:
Length of the log 8 Radius of the log 0.7 Velocity of the conveyor 1
Time step 0.06
Table 5: Parameters of the model of the motion of the log
According to parameters in the Table 5, the whole log could be scanned in 8 seconds and measurements are made every 60ms. Thereby, 134 image reconstructions of slices of the log are obtained during one full scanning process.
6 IMPLEMENTATION 36
6.2 Numerical experiments
We have performed a series of tests with dierent initial conditions and presence of strike against the second conveyor. In other words, two types of tests are prepared:
- Simulations when log strikes against the second conveyor
- Simulations when log moves smoothly without unexpected jumps
Figure 10: Cartesian coordinate system of PhysX environment
Let us rst introduce the test to be used to verify the model under study. The analysis does not involve a jump of the log. For this test, measurements are obtained from a simulation of the motion of the log where the initial position of the log diers from the initial state of the body in the Kalman algorithm. In other words, error in initial state is considered. We study that case because the logs can be laid on the conveyor randomly around the center of it. To understand current experiment Cartesian coordinate system of the PhysX environment is illustrated on Figure 10.
We assumed that the timber is rotated a bit around y-axis and shifted to the positive
direction of x-axis. Next, we introduce the evolutionary model of the extended Kalman lter. As it was mentioned earlier, the synthetic data is also produced by PhysX. However, their accuracy diers from each other. The evolutionary model is being calculated with smaller time step 0.02. Thus, the model of the Kalman lter is biased. Therefore, it is possible to simulate the modeling error, which always appears since all the models, generally, provide approximations for real phenomena.
The evolutionary model noise was assumed to have zero-mean normal distribution and independent components with standard deviation:
Ck =σcI,
where σc= 10−3 and I is identity matrix.
The distribution of initial state values is also assumed to obey multivariate normal distribution with zero expectation and following standard deviation:
P0|0 =σP0I, where σP0 = 10−3 and I is identical matrix.
Actually, standard deviations for initial state values, process noise, and observation noise are arbitrary. However, it is worth mentioning that convergence of the Kalman lter was achieved with dierent noise levels as well.
The result of the implementation of the extended Kalman lter to the present test is illustrated on Figure 11. There was an initial large mean squared error due to inaccurate guess regarding initial position only. As we can see the mean squared error between the estimates obtained from the ExKF estimator and the state of the true model declines gradually so the present approach minimized mean squared error successfully. What is more, the speed of convergence can be tuned by changing the initial guess regarding variance of initial state distribution. According to the lower graph on Figure 11, the error in x coordinate and in orientation were minimized.
However, it is impossible to estimate the position of z coordinate (along the conveyor) exactly since X-ray tomography provides information only about a slice of wood.
Therefore, the error in the z-coordinate of log decreased only to some extent. Other components of state vector were estimated with low error. In addition, the Kalman algorithm run in real time. In other words, every iteration of the Kalman lter was computed before new measurements were obtained. Numerically, it means that one iteration takes less than 60 ms. Actually, computational time of one step of the Kalman lter takes 3ms in average excluding time for making an observation.
6 IMPLEMENTATION 38
Figure 11: Mean squared error between the estimates obtained from the ExKF estimator and the state of the true model. Upper: Total MSE, Lower: contributions from components.
Unfortunately, changing the state of the body by the Kalman lter can lead to unphysical positions of the log, to the penetration of objects. PhysX software elim-inates this by applying impulses to penetrated objects. This resolution of the pen-etration will cause the log to y o or at least to jump up to the air. Thereby, the Kalman lter is subject to disconvergence due to accidental intersections of log and conveyor. In order to illustrate this situation we performed the same test, but with
Figure 12: Mean squared error between the estimates obtained from the ExKF estimator and the state of the true model. Upper: Total MSE, Lower: contributions from components.
dierent assumption regarding noises of the model. It can be seen from the graph on Figure 12, how after 80th measurement the movement of the body becomes un-stable because of inappropriate estimation of timber's position in terms of PhysX.
Therefore, a special method for avoiding unphysical instabilities is required. We solved the issue by saving the current state of the body and further evolving several future steps of PhysX simulation. When a dramatic change of the state occurs we
6 IMPLEMENTATION 40 go back to saved state and modify it so that the penetration between the body and conveyor is eliminated. The present approach does not consume a lot of processing time.
Figure 13: Mean squared error between the estimates obtained from the ExKF estimator and the state of the true model. Upper: Total MSE, Lower: contributions from components.
In addition, it was noted that observations obtained from the middle part of the log in case of log rotation can be misleading for the Kalman lter. It can be explained as follows. The central point of the log is extreme since some elements of the Jacobian
of observation operator change sign when crossing that point. As a result, the error in z-coordinate induces error in Jacobian. Thereby, we decided not to take into account observations obtained in that region. On Figure 13, the result of the implementation of modications to the same test which was introduced earlier could be seen. It is worth mentioning that current approach provides approximately the same result as for the rst test on Figure 11. What is more, the computation time of one iteration increased by just 2ms.
Next, we decided to apply the Kalman lter to the data obtained from the simulation with the jump. We assume rst that the initial states of the true model and the Kalman lter coincide. The true model for this case was modied so that an impulse was applied to the log when it touched the second conveyor in order to simulate the jump of the log. Clearly, the evolutionary model should be adapted to detect the jump. We decided to give the additional impulse by PhysX to the timber when observation reveals a jump. The presence of jump can be deduced from the changing of the center of the ellipse, which is the reconstruction of the slice of the log. On Figure 14, the mean squared error of the state estimation by the Kalman lter could be seen. At rst iterations, the Kalman lter provides precise enough estimation of state, since initial guess of the position of the body was consistent with its true state. However, Figure 14 indicates that after the jump the algorithm is not able to predict the z-coordinate of the state properly. Nevertheless, this approach can predict the movement of the log regarding y-coordinate(see Figure 15).
6 IMPLEMENTATION 42
Figure 14: Mean squared error between the estimates obtained from the ExKF estimator and the state of the true model. Upper: Total MSE, Lower: contributions from components.
Figure 15: y-coordinate of the body's state for the estimates obtained from the ExKF estimator and the states of the true model.
Figure 16: Total mean squared error between the estimates obtained from the ExKF estimator and the state of the true model.
6 IMPLEMENTATION 44 In addition, we decided to solve the same problem without applying additional impulse by PhysX in case of the jump in order to compare their accuracy. In other words, the model with jump was simulated by the model without it. On Figure 16, we can see that the estimation is twice higher compared to the previous model.
Therefore, we can conclude that our approach provides better result comparing to simple movement of the log along the conveyor belt.
Finally, we applied our extended Kalman lter to a more complex problem. Here the true model and our model have dierent initial states and also the jump occurs.
As we can see, Figure 17 reveals that the mean squared error declines until the jump. However, the jump contributes to increasing the estimation error since it is assumed as an unpredictable movement. Nevertheless, the current method is capable of estimating the state of the timber appropriately so that the dierence between true state and estimated state is minimized. However, the error in prediction of z-coordinate of the position of the body remains in the estimations to the end of the process. Clearly, the performed analysis of the examined test cases reveals that our approach demonstrate robustness and high performance for the problem under study, except for the z-coordinate.
Figure 17: Mean squared error between the estimates obtained from the ExKF estimator and the state of the true model. Upper: Total MSE, Lower: contributions from components.
7 CONCLUSIONS AND FUTURE PLANS 46
7 Conclusions and future plans
In this study, an extensive analysis of the application of the extended Kalman lter to the problem of estimation of the position of the log on a moving conveyor belt was performed. We found out that the position of the log can be deduced from X-ray computer tomography images. The most crucial part of the study covered sucient modeling for simulating the rigid body's motion with the help NVidia PhysX SDK.
In addition, the algebraic reconstruction approach for X-ray tomography was im-plemented. It is worth mentioning that CUDA parallel platform was used to speed up the performance of the computer tomography algorithm. As a side achievement of the present research we showed that unexpected movement of rigid bodies due to their penetration can be eliminated. The present study was implemented as Matlab
In addition, the algebraic reconstruction approach for X-ray tomography was im-plemented. It is worth mentioning that CUDA parallel platform was used to speed up the performance of the computer tomography algorithm. As a side achievement of the present research we showed that unexpected movement of rigid bodies due to their penetration can be eliminated. The present study was implemented as Matlab