Rigid body dynamics and Kalman filtering

(1)

Computational Engineering

Konstantin Taranov

RIGID BODY DYNAMICS AND KALMAN FILTERING

Examiners: Professor, Ph.D. Heikki Haario

Associate Professor, Ph.D. Joonas Sorvari

(2)

ABSTRACT

Lappeenranta University of Technology LUT School of Engineering Science Computational Engineering

Konstantin Taranov

Rigid body dynamics and Kalman ltering Master's thesis

2015

51 pages, 17 gures, 5 tables

Examiners: Professor, Ph.D. Heikki Haario

Associate Professor, Ph.D. Joonas Sorvari

Keywords: Kalman ltering, Physx, rigid body dynamics, X-ray computed tomography

X-ray computed log tomography has always been applied for qualitative reconstructions. In most cases, a series of consecutive slices of the timber are scanned to estimate the 3D image reconstruction of the entire log. However, the unexpected movement of the timber under study inuences the quality of image reconstruction since the position and orientation of some scanned slices can be incorrectly estimated. In addition, the reconstruction time remains a signicant challenge for practical applications. The present study investigates the possibility to employ modern physics engines for the problem of estimating the position of a moving rigid body and its scanned slices which are subject to X-ray computed tomography. The current work includes implementations of the extended Kalman lter and an algebraic reconstruction method for fan-bean computer tomography. In addition, modern techniques such as NVidia PhysX and CUDA are used in current study. As the result, it is numerically shown that it is possible to apply the extended Kalman lter together with a real-time physics engine, known as PhysX, in order to determine the position of a moving object. It is shown that the position of the rigid body can be determined based only on reconstructions of its slices. However, the simulation of the body movement sometimes is subject to an error during Kalman lter employ- ment as PhysX is not always able to continue simulating the movement properly because of incorrect state estimation.

(3)

First of all, I would like to express my gratitude to Lappeenranta University of Technology for providing the opportunity to study and carry out the research within the precincts of the university.

I owe my deepest gratitude to my scientic supervisor Heikki Haario for giving valuable advice and comments which have been a great help for the current project.

I am particularly grateful for the assistance given by Aleksandr Bibov who stated directions of this work, gave insightful comments and suggestions.

Finally, I would like to express my special appreciation to my beloved girlfriend and my family for support and warm encouragement. Without their help this research would have never been accomplished.

Lappeenranta, September, 2015.

Konstantin Taranov

(4)

CONTENTS 4

1 INTRODUCTION

Wood is a valuable raw material that appears visually as a very homogeneous mass.

However, the factors aecting the value of log lie under its surface and are revealed only after sawing. The forest products industry requires a deep knowledge of internal properties of stems and logs for several industrial processes. One of the possible approaches is a computed tomography technology. Therefore, scanning of internal properties of stems and logs has been a signicant problem for proper and accurate X-ray computed tomography reconstruction. The main reason, which makes this class of problems so important, is that accurate reconstruction of logs can be used for characterisation of wood raw material. Moreover, knowledge of the position and the size of internal log defects, such as knots and decays, is required for optimizing log sawing that brings lumber value improvement, ranging from 3% to 28% depending on the species [1, 2].

For accurate control of product quality, one needs real-time scanning of the inner structure of wood properties. In other words, the reconstruction time remains a signicant challenge for practical implementations. Nowadays, this problem is widely solved by employing fast reconstruction algorithms and powerful hardware [3]. In most cases, all algorithms are based on gathering the data which were obtained from a series of consecutive slices of the timber and further estimating the 3D image reconstruction of the entire log. However, the unexpected movement of the scanned timber inuences the quality of image reconstruction. Usually, this movement appears as a result of a stroke against a conveyor belt. Since the scanned object experienced unpredicted shift, the position and orientation of some scanned slices are incorrectly estimated. Thereby, these inaccuracies lead to an error in estimating the 3D reconstruction of the timber.

The analysis of the movement of logs is an example of dynamical state estimation problem. One of the most popular methods for solving dynamical state estimation problems is known as the Kalman lter [4]. It is a powerful tool for a wide range of problems. Kalman ltering is an optimal estimator of a dynamically changing state of the system that minimizes the mean-square error between measurements and model values. In addition, the classical Kalman lter algorithm shows high performance for small problems, such as a problem of estimating the position of the rigid body. However, it requires an appropriate and accurate model for the timber movement. The movements and interactions of rigid bodies are studied by rigid-body dynamics. It is able to provide an equation of the exact changing of

(8)

1 INTRODUCTION 8 the position of the body if all forces are taken into account and specied. Thereby, calculating the exact solution is time-consuming. Instead of exact solutions, rigid body dynamics is approximated by physics engines, which are computer software that solve rigid body dynamics problems. One of the fastest and the most accurate engines is NVidia PhysX.

In case of the Kalman lter, many research papers are devoted to the dynamical state estimation of the position of rigid bodies. There are also studies carried out on specic sensors like gravity and angular ones. For example, Joao Luis Marins et al. [5] studied the implementation of the extended Kalman lter for estimation of the rigid body orientation using magnetic, angular, rate, and gravity sensors. They proposed the use of quaternions for dening the orientation of the body, whereas Ligorio, Gabriele, and Angelo Maria Sabatini [6] suggested to employ Euler angles for that purpose. Moreover, the application of the indirect Kalman lter was studied by Trawny, Nikolas, and Stergios I. Roumeliotis [7], who also employed quaternions for estimating the position. In case of real-time physics engines, many scientists have already used PhysX in their research. Killpack, Marc D [8] suggested to utilize PhysX and the extended Kalman lter for soft body dynamics problems. However, the combination of the Kalman lter algorithm and PhysX has never been studied before for solving the problem under study.

The purpose of this study is to investigate the possibility of employing modern physics engines for the problem of estimating the position of a moving rigid body and its scanned slices which are subject to X-ray computed tomography. The current work includes implementations of the extended Kalman lter and an algebraic reconstruction method for fan-bean computer tomography. In addition, the implementation time is a signicant challenge for the research. Hence, a parallel computing platform, known as CUDA, is used to boost up the performance.

The structure of this Thesis is as follows. The next section briey goes through the theoretical background for sawmill industries and how they utilise X-Ray computer tomography. Section 3 covers denitions and concepts of the Kalman lter and the extended Kalman lter algorithms. Sections 4, 5 move on establishing of state-transition model and observation model respectively for implementing of the extended Kalman ltering. Section 6 imposes a series of tests to be performed in order to verify consistency of the model under study. Finally, section 7 concludes and gives proposals for future work.

(9)

2 Background

2.1 Sawmill industry

Cross-cutting of tree stems is a crucial part of wood raw material processing. Mostly, sawmills are massive and expensive facilities where most aspects of the work are computerized. They utilise forest resources by producing sawn timber to be used in construction, manufacturing of furniture, windows and doors etc. Sawmills also supply pulp mills with high-quality chips.

Wood is a valuable raw material and appears visually as a very homogeneous mass.

However, the factors aecting the value of log lie beneath its surface and are revealed only after sawing. Hence, the quality of a nal product can be uncovered only after sawing the wood. An ecient usage of the raw material requires the knowledge of the factors aecting the value hidden under the bark. There are several parameters describing geometric and quality features of logs. Typical quality features are knots, annual ring width, rotten etc.

Figure 1: Value yield as function of top diameter for cant sawing, live sawing and component sawing methods [9].

Conventional scanners, based on laser technology, are capable of providing information about geometrical features of logs only, i.e. top diameter, length, sweep and ovality. A big step in scanning technologies is the implementation of X-ray

(10)

3 KALMAN FILTER 10 inspection systems, which are the only non-destructing measuring technique which allows to reveal also internal properties of round wood. Typical internal features of timbers include the dimensions/volume, moisture content, volume of knots, rot and other defects and heartwood/sapwood ratio. According to FlexWood [9], log scanner systems for measuring the shape and internal properties of logs are widely used by sawing industries in the following processes:

• Log sorting station,

• Bucking and cross cutting terminal,

• Just before sawing,

• Harvesting machines.

Knowledge of the position and size of internal log defects, such as knots and decay, is required for optimizing log sawing that brings lumber value improvement, ranging from 3% to 28% depending on the species [1, 2]. Figure 1 shows comparison between three dierent sawing methods by the FlexWood project [9]. Live and cant sawing methods are classical approaches for timber cutting. They do not take into account inner properties of timbers, in other words they are based on geometrical features only. In contrast to this, component sawing method is an approach which utilize internal properties of round wood. It calculates the optimal log rotation angle and size of wooden bars in terms of maximum yield and saw the timber after that.

According to Figure 1, the best value yield is achieved by using the component sawing method.

3 Kalman lter

3.1 Basic Kalman lter

Dynamical state estimation problems often appear in industries. The problem of determining the position of a log on the moving conveyor belt is an instance of such kind of problem. One of the most popular methods for solving dynamical state estimation problems is derived from the system of Kalman equations and is known as Kalman lter [4]. It is a powerful tool for a large class of problems. Kalman ltering is an optimal estimator of the dynamically changing state of the system

(11)

that minimizes the mean-square error between measurements and model values. A linear dynamical state estimation problem can be formulated as follows. Estimate the state x_k ∈Rⁿ of a discrete-time controlled process

x_k=F_kxk−1+B_ku_k+w_k using a series of measurementsy_k ∈R^m observed over time

yk =Hkxk+vk.

Here,F_kis a state-transition model,B_kis a control-input model,H_kis an observation model. The error terms wk and vk represent the process and measurement noises respectively. They are typically assumed to be independent with multivariate normal probability distributions:

w_k ∈N(0, C_k), vk∈N(0, Rk).

Essentially, the model approximates the evolution of the system with some accuracy. Moreover, sensors are likely to provide noisy data. For these reasons the Kalman lter was proposed to calculate a weighted average of a measurement and a prediction of the system under study. The new state estimation has a better estimated uncertainty than either of them. The process is continued every time step to calculate how the state of the system alters with respect to time. The lter is recursive and requires for calculation just the last state and new measurement.

In other words, the new state of the system does not rely on old states of the system.

The basic Kalman lter algorithm can be implemented in two steps. The rst step is to predict a priori state estimate and its priori estimate covariance. This step is also called prediction step. The second step includes combining the priori state estimate with the observation by computing optimal Kalman gain matrix. This improved estimate is called the a posteriori state estimate. At time step k the Kalman lter algorithm estimates state vector x_k and its covarianceP_k with givenx_k−1,P_k−1,F_k, B_k , H_k ,y_k,C_k,R_k.

The basic Kalman lter algorithm reads as follows:

(12)

3 KALMAN FILTER 12

Prediction

a priori state estimate xk|k−1 =F_kxk−1|k−1+B_ku_k a priori estimate covariance Pk|k−1 =F_kPk−1|k−1F_k^τ +C_k

Update

Here, the notationxn|m represents the estimate ofxat timen given observations up to, and including at time m≤n.

3.2 Extended Kalman lter

When either the system state dynamics or the observation dynamics is nonlinear, the extended Kalman lter [10] can be adopted, which implements a Kalman lter for a system dynamics that results from the linearization of the original non-linear lter dynamics around the previous state estimate. Unlike its linear version, the extended Kalman lter, in general, is not an optimal general state estimation problem. It is formulated as follows:

x_k = f(xk−1, uk−1) +w_k, y_k = h(x_k) +v_k,

where f and h are nonlinear. Therefore, f and h cannot be applied to the covariance formulas directly. However, their linear approximations can be estimated by calculating the Jacobian matrix. Thus, the extended Kalman lter directly uses the Kalman lter formulas in the nonlinear case by replacing the nonlinear model and observation operators in the covariance computations with their linearization:

F_k = ∂f

∂x

xk−1,k−1,uk−1

,

Hk = ∂h

∂x _x

k,k−1

.

The basic extended Kalman lter algorithm reads as follows:

(13)

Prediction

a priori state estimate xk|k−1 =F_kxk−1|k−1+B_ku_k a priori estimate covariance Pk|k−1 =F_kPk−1|k−1F_k^τ +C_k

Update

4 State transition model

4.1 Rigid body dynamics and physics engines

Rigid-body dynamics studies the movement and interaction of systems of interconnected solid bodies under the action of external forces. The assumption that bodies are solid is extremely crucial for analyzing the motion of bodies. Under the assumption, objects are assumed not to be able to be deformed by external forces. As a result, it leads to a simplication of modeling and simpler forms of equations which describe the conguration of the system. As a matter of fact, dynamics of a solid body can be dened by a set of dierential equations. The solution of the equations describes the motion of the body as a function of time. Mechanical systems often consist of a wide range of complex interconnected solid bodies, which leads to a system of equations usually solved by numerical methods such as nite element method.

A physics engine is a computer software that solves rigid body dynamics problems.

Physics engines provide an approximate simulation of certain physical systems by employing numerical methods. They are mainly used for simulation of solid body or liquid motions. They are good not only at numerical simulations of rigid body dynamics, but also at simulation of soft body dynamics, and uid dynamics. Physics engines can be categorized into two groups: real-time and high-precision engines.

High-precision physics engines are used to calculate rigid motions with high accuracy. Thus, they require more processing power and are usually used by engineers and scientists. In contrast, real-time physics engines were designed to calculate rigid body dynamics as fast as possible, so as to calculate simulations in real time.

Therefore, they use simplied calculations and are inclined to have limitations in

(14)

4 STATE TRANSITION MODEL 14 simulating body motion.

4.2 NVidia PhysX

NVidia PhysX [11] is a real-time physics engine. PhysX is one of the physics engines used in a wide range of modern computer games. PhysX technology belongs to NVidia Corporation and can be used only on modern NVidia GPU devices. It supports all popular operation systems such as Windows, Mac OS X, Linux and some home video game consoles: PlayStation 3, Xbox 360 and Wii.

PhysX approximates real world physics with single-precision oating point numbers.

For many applications, it provides precise enough and high-performance simulation of rigid body dynamics. PhysX, in addition, computes simulations in "quasi-real time". It means that a simulation of rigid body dynamics in PhysX takes less time than it takes in a real world process.. Clearly, that statement depends on computing power of a device and complexity of a process. However, good performance induces limitation in PhysX functionality. First of all, time in a PhysX simulation is discrete, not continuous. It means that the state of the simulated system is known only at calculated time points, known as steps. Next, the simulation process evolves only forwards in time. In other words, PhysX asserts that if the state of a system is known at a given time step then PhysX can calculate the state of the system at future time steps only, not past.

A simulation of rigid body dynamics in the PhysX environment is straightforward (see Figure 2). First, a scene should be created. One of the main properties of the scene is gravity. It is determined by gravitational acceleration and direction of gravity. Clearly, it is possible to simulate body interactions in dierent circumstances of the planet. As the scene is determined, any objects, known as actors, can be added to the simulated world. The position of an actor is represented by a transform. A transform describes the transition of an actor from its initial state and its rotation around the origin. There are two types of actors: dynamic and static (Figure 2).

Dynamic and static actors have common properties such as shape and position.

Static actors are immovable during the simulation. They can interact with other objects but cannot change their position. In contrast, dynamic actors take part in simulation and are inuenced by forces. Therefore, they have additional properties such as mass, velocity, torque, center of gravity and others. The main property of

(15)

Figure 2: Concept of PhysX's environment [12]

rigid bodies is that they always retain they original shape when forces are applied.

Material properties of a physical object dene static friction, dynamic friction and restitution. Restitution denes bounciness of the object and has a value between 0 and 1. Static friction is friction between two or more solid objects that are not moving relative to each other and its value can vary from zero to innity. In contrast to this, dynamic friction is applicable to two rigid bodies that are moving relative to each other and rub together. It appears only when moving, and its value is always within 0 and 1.

The shape is determined by geometry and material. PhysX has a few geometry primitives such as box, capsule, and sphere. Unfortunately, there is no cylinder among primitives, which perfectly describes the shape of a log. However, PhysX gives opportunity to create a mesh for a geometry. Therefore, one of the rst steps in current research was to code a function for creating cylinders with given radius and length.

Finally, simulating a rigid body dynamics in PhysX environment requires calculating the new position of all actors which are under the eect of Newton's law for every time frame. The time between the frames, also known as the time step, must be specied. The size of time step inuences the accuracy of the simulation.

(16)

4 STATE TRANSITION MODEL 16 Thereby, the body movement, which is calculated with some value of time step, may dier from the movement of the same body, which was simulated with dierent size of time step.

4.3 A model of the motion of the log

In our research, we adopt PhysX to simulate the motion of a log. To test performance and reality of rigid body dynamics simulation we decided to design two PhysX environments with dierent concepts. In the rst environment, a moving log and a conveyor were designed as moving cylinder on two rectangular parallelepipeds with a gap between them (Figure 3). Movement of the cylinder along the conveyor is simulated by assigning a speed to the cylinder. The strike at second conveyor is simulated by a contact event, which sets a force toward a part of the cylinder when the log touches the second conveyor. In the second environment, the conveyor was

Figure 3: The rst model of simulation of log's motion

designed as a series of roller conveyors with gaps between them. Every roller has its own angular velocity around its own axis. In that case, rollers impart speed to the cylinder. Similar to the rst simulation, a strike is simulated as a contact event between the log and the rst roller of the second conveyor.

(17)

Figure 4: The second model of simulation of log's motion

Processing time Time step Frames Time interval

(in seconds) (in seconds) (in seconds)

14.322 0.02 1000 20

15.377 0.015 1000 15

Table 1: Performance tests for the second environment

Processing time Time step Frames Time interval

(in seconds) (in seconds) (in seconds)

0.711 0.01 1000 10

0.689 0.005 1000 5

0.802 0.001 1000 1

Table 2: Performance tests for the rst environment

(18)

4 STATE TRANSITION MODEL 18 From the description of two simulations we can conclude that the second one is more complex. Therefore, it will likely require more processing power. To estimate the performance, we decided to assess the calculating time of 1000 frames with dierent time steps. As we can see the roller based simulation is quasi-real for time step 0.02 second, but for a shorter time step of size 0.015 second the simulation takes more time than the real-world process under study. Unlike the second simulation, the rst one is able to run in quasi-real time even for a small time step of 1ms. Therefore, we decided to perform the simulation by utilizing the rst model.

4.4 Evolutionary operator

Let us rstly introduce quaternions in order to use them for specifying the evolutionary operator. Quaternion is a number system that extends complex numbers.

They were rst introduced by William Rowan Hamilton. Unlike complex numbers, quaternions have three imaginary terms. Thereby, multiplication of two quaternions is a noncommutative operation. Since quaternions are an extension of complex numbers, they inherit some properties of complex numbers, such as norm, conjugation, unit norm and inversion. A quaternion q is dened as

q =ix+jy+kz+w, x, y, z, w ∈R,

(1)

with Hamilton's rules

i² =j² =k² =ijk =−1

ij =k, jk =i, ki=j, (2)

ji=−k, kj =−i, ik=−j.

Hamilton's rules (2) state that the occurrence of ij is replaced by k, jk by i, kiby j, and etc. The rules are very similar to the cross product of Cartesian vectors. For more information regarding quaternions see "Quaternions for computer graphics"

by John Vince [13].

To dene a dynamical state estimation problem, it is necessary to choose the state vector space and specify its state-transition model. As it was mentioned earlier, the goal of this research is to analyze the position of a body in space. We decided to adopt transform as spatial variables of the body.

(19)

Transform in PhysX is a vector which contains seven values. The rst three components represent a displacement from the initial position and the last 4 components represent a rotation, which is dened as a unit quaternion. It is worth mentioning that the rotation can also be expressed as a rotation matrix. The matrixP is called rotation matrix if it is a square matrix with real entries whose columns and rows are orthogonal unit vectors i.e.

P^τP =P P^τ =I,

where I is the identity matrix. In other words,P should be an orthogonal matrix.

For a number of reasons, a quaternion is a better way to represent the orientation of a rigid body than using a classical 3x3 rotation matrix [14]. The rst reason is that usage of the rotation matrix in rigid body dynamics is subject to numerical error. Therefore, a numerical rotation matrix will no longer be precisely a rotation matrix, i.e. orthogonal, which will result in a skewing eect. The next reason is that a quaternion represents a rotation by 4 points, whereas a rotation matrix contains 9 points. Therefore, the degree of redundancy is noticeably lower for quaternions than for rotation matrix. Quaternions are subject to numerical error too, so it is preferable to use normalized quaternions. A quaternion is called normalized if it satises equation (3). Therefore, numerical errors in quaternion could be easily correctable by renormalization.

x²+y² +z²+w² = 1 (3) It is worth mentioning that a rotation matrix can be calculated from a quaternion.

If a quaternion has the form (1) and is normalized by (3), it can be easily translated into a left-handed rotation matrix:

P =







1−2y²−2z² 2xy−2zw 2xz+ 2yw 2xy+ 2zw 1−2x²−2z² 2yz−2xw 2xz−2yw 2yz+ 2xw 1−2x²−2y²







The resulting matrixP will be always a rotation matrix, i.e. orthogonal. A compos- ite rotation is specied by quaternion multiplication. Therefore, using quaternions is as simple as using rotation matrixes. It means that two consecutive rotations P1

and P2 can be combined as P3 = P2P1. In the quaternion representation it has a

(20)

4 STATE TRANSITION MODEL 20 form q₃ =q₂q₁, where q₁ and q₂ are quaternions which are associated with rotation matrixes P₁ and P₂ respectively.

In addition, quaternions can be represented as matrices. It means representing quaternions as matrices in such a way that quaternion addition and multiplication correspond to matrix addition and matrix multiplication. 4×4real matrices can be used for such representation:

Q=x







0 0 0 1 0 0 −1 0 0 1 0 0

−1 0 0 0





 +y







0 0 1 0 0 0 0 1

−1 0 0 0 0 −1 0 0





 +

+z







0 −1 0 0

1 0 0 0

0 0 0 1

0 0 −1 0





 +w







1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1







=







w −z y x z w −x y

−y x w z

−x −y −z w







In this representation, conjugation of quaternion corresponds to transposition of matrix. When a quaternion is normalized then matrix Q is a rotation matrix in 4-dimensional space. It means Q is an orthogonal matrix and Q⁻¹ =Q^τ.

As was mentioned before, state vector has a form of the transform. First three numbers represents displacement from the origin and last 4 numbers represent rotation which is dened as a unit quaternion:

(21)

~ x=





 c1

c₂ c3

x y z w







(4)

In current research we construct evolutionary operator from 2 operators: one for a position of the body and one for a rotation of the body. Its Jacobian will have a form

F =





A 03×4

04×3 B



.

where A and B are the Jacobians of evolutionary operators for position and orientation, respectively.

As it was mentioned earlier, the extended Kalman lter denes the state transition model as a function which is applied to the previous state to calculate the current one. Therefore, we want to express the position of the object at time t_k+1 from the position of the object at time t_k. Clearly, the evolution of a position can be written asx_k+1 =P x_k+D, where P is a rotation andDis a displacement. That kind of evolution is nonlinear because of the displacement, and its Jacobian is equal toP. Now we are going to nd A, which equals to P: PhysX is able to provide the position of a body at time t_k as a rotation of initial position of the body and its displacement. Hence, we can dene the position of the body at time t_k as:

xk+1=Pk+1x0+Dk+1. (5) Similarly, we can dene x_k:

x_k =P_kx₀ +D_k⇒x₀ =P_k^τ(x_k−D_k). (6) Now we can derive equation for x_k+1 by substituting (6) into (5):

x_k+1 =P_k+1(P_k^τ(x_k−D_k)) +D_k+1 =P_k+1P_k^τx_k−P_k+1P_k^τD_k+D_k+1 (7) Thus, the evolutionary operator has the form (7) and its Jacobian A equals to P_k+1P_k^τ. In the same manner we are able to calculate evolutionary operator for

(22)

5 OBSERVATION MODEL 22 quaternion. The rotation of object at time t_k+1 can be dened as a rotation of its initial state, which is provided by PhysX. As it was mentioned before, a quaternion can be represented as a 4×4 matrix. Therefore, we can express the evolutionary operator in matrix form:

q_k+1=Q_k+1q₀ q_k=Q_kq₀

⇒qk+1 =Qk+1Q^τ_kqk. (8)

Here Q_k is a matrix form of a quaternion. Hence, the evolutionary operator for quaternion B is linear and equals to Qk+1Q^τ_k.

Combining (7) and (8), we can obtain nal form of Jacobian of evolutionary operator:

F =





P_k+1P_k^τ 03×4

04×3 Q_k+1Q^τ_k



.

5 Observation model

5.1 Computed tomography

Computed tomography is a procedure that combines a series of X-ray images taken from dierent angles into a cross-sectional image of a specic area of the scanned object. It is used to reveal inner properties of an object without destroying it.

Application of computed tomography takes places commonly in medicine and non- destructive material testing. When X-ray beam comes through the sample, its intensity attenuates because of absorption. The nal intensity is measured with a detector at the other side. Attenuation depends on the particular paths of beams, so it is necessary to scan the object from all directions in order to determine the inner structure. The process of reconstructing the image from sensors' data relies on knowledge from physics, mathematics and image processing.

(23)

5.2 Lambert-Beer's law

Lambert-Beer's law describes light absorption and the properties of the homogeneous object through which the light is traveling. It states that the quantity of light absorbed by a substance is directly proportional to the concentration of the substance and the path length of the light through the object. This relationship was discovered by J.H. Lambert in 1760 and extended by A. Beer in 1852. Mathemati- cally, it can be written as:

I(η) =I₀e^−µη, (9)

where I is an intensity of the beam, µ is an attenuation coecient and η is the thickness of an object.

The penetrated material can be non-homogeneous, so its attenuation coecient varies depending on particular place inside the object. In that case, equation (9) changes to the form:

I(η) =I0e⁻

Rs 0 µ(η)dη

, (10)

where µ(η) is a spatially varying attenuation, s is a length of the beam.

The image reconstruction of the particular slice of the body involves discretization of the material. The material can be described as a set of dierent partitions with given lengths and attenuation properties. In that case, Lambert-Beer's law (10) can be rewritten as:

I(η) = I₀e⁻^Pⁿⁱ⁼¹^µⁱ^ηⁱ. (11) From (11) we can nd the projection integral in continuous cases, or the projection sum in discrete cases, which describes attenuation coecient of a given slice of the object along the X-ray path.

p=−ln I(η)

I₀

=

n

X

i=1

µ_iη_i (12)

(24)

5 OBSERVATION MODEL 24

5.3 Algebraic reconstruction method

Algebraic reconstruction method is an intuitive method for scanning an object. The rst image reconstructions were created with the help of this method. Algebraic reconstruction method represents the reconstruction problem as a linear system of equations. Our simulation of X-ray scanning involves Fan-beam projection techniques. It means that every source of radiation cast X-rays along paths which describe a fan. There are detectors opposite to the source of X-rays which record the intensity of the beam after passing the sample (Figure 5). By comparing the initial and nal intensities we can conclude what is hidden under the surface of the sample.

Figure 5: Fan-beam CT imaging geometry.

Algebraic reconstruction method is based on a discretization of an active domain.

The active domain is a region of space which is scanned by tomography. This discretization of the domain is essential because the tomography image has discrete structure. In addition, their resolution will coincide. The tomographic image dis-

(25)

cretization has to be determined before reconstruction step. Every pixel represents a region of an object with its own attenuation coecient. The main goal of algebraic reconstruction method is to determine the attenuation coecient of every pixel.

Clearly, the smaller the discretization size is, the higher resolution of the image is received, in principle. However, high-resolution tomography images require more processing power.

When X-ray beam comes through the active domain, intersected pixels absorb its intensity. The resulted intensity is measured with a detector at the other side. Un- known attenuation coecients we will denote by f_j. From (12), the projection can be calculated by:

p=−ln I(η)

I₀

=

n

X

i=1

f_iη_i (13)

Figure 6: A beam penetrates the active domain.

All X-ray beams induce a linear system of equations. Clearly, the number of beams should be greater or equal to the number of unknown attenuation coecients. If the image is small, the solution of a linear system of equations can be calculated with the help of Gaussian elimination.

In the system (13) values η_i is still undetermined. They represent the length of an intersection of a pixel and a beam, which is determined by a straight line. When an X-ray is passing through an object, this length has to be taken into account.

Let us denote weight a_ij as length of intersection j pixel with ray i. From (9)

(26)

5 OBSERVATION MODEL 26 one obtains the following system of equations, where N is number of pixels:

Figure 7: A beam(blue line) passes through limited number of pixels(yellow squares).

a₁₁f₁+a₁₂f₂+. . .+a_1Nf_N = p₁ a₁₁f₁+a₁₂f₂+. . .+a_1Nf_N = p₂

...

a₁₁f₁+a₁₂f₂+. . .+a_1Nf_N = p_M

In vector form it can be rewritten as

Af =p. (14)

It is worth mentioning that (14) can be solved exactly only under restricted conditions. Usually, we consider only approximate solutions. In practice, the number of projections should exceed the number of unknown variables. Mathematically, it leads to an over-determined system of linear equations.

When a beam comes through an object, it passes through a limited number of pixels. Mathematically, it means that in every equation just a few elements appear.

Therefore, matrix A is almost singular and sparse. In addition, it is considerably large and cannot be stored completely. Figure 8 gives a sparsity pattern.

(27)

Figure 8: Sparsity pattern of theory matrix.

The solution of the problem (14) can generally be rewritten as the following minimization problem:

fmin∈RN

|Af −p|²

There is a plenty of methods for solving that kind of problem, such as SVD decompo- sition, maximum likelihood methods and iterative minimization methods [15]. The problem can be solved by Barzilai-Borwein gradient method, which showed good results in its implementation for computer tomography [16]. This algorithm is notable for fast convergence and a quite precise reconstruction.

5.4 A model of a X-ray scanner

As it was mentioned before, a scanning process of the particular slice of the log includes simulation of the body movement. The simulation provides the position of the body and its rotation. Beforehand the geometry of the log should be dened, as well as position of the X-ray sources and detectors. Based on the position of the log and the scanners, one can deduce whether radiant beams intersect the log. When the log does not interact with the scanning region the densities of pixels are equal to 0. The densities of the log are stored as a three-dimensional matrix where every single element represents its inner parts. Since the densities in initial position are specied and the position of the log is known one can calculate densities which lies inside the slice of the body which is penetrated by X-rays.

(28)

Let us denote by b_j the coordinate of the pixel which corresponds to attenuation coecientf_j. According to the description of the position of the log, the relationship between scanned pixels of the active domain and density's nodes b_j of the log can be expressed as follows:

p_j = ¯P b_j + ¯D,

where P¯ is rotation in the index space and D¯ is displacement in the index space.

Rotation and displacement which are determined in PhysX environment can always be transformed into another space by multiplying them on corresponding matrixes.

Since the coordinate systems of PhysX and a matrix dier, we change the basis of our rotation and displacement by multiplying them on appropriate transformation matrix. In addition, we inverse our rotation and displacement to calculate indexes of densities of the log lay in the scanned region:

b_j = ¯P^τ(p_j−D).¯ (15)

It is worth mentioning that the coordinates of scanned pixels of the active domain are integers. Thereby, we are able to enumerate all possible cases for indexing attenuation coecient f_j. Unlikep_j, coordinates of density's nodes after evaluating (15) will have real values. As densities of the log are stored in computer memory as a three-dimensional matrix with integer indexes, trilinear interpolation to log's data is needed to calculate densities which lie between dened nodes of the log. Applying rotation and interpolation to every single node is costly. To speed up this process we adopt CUDA technology for parallel execution of code on GPU cores.

5.5 Observation operator

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

(29)

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

(30)

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 innite 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|² .

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 V⁰ =GV is in the plane with normal vector D and origin. The projection matrix has few properties. The main one is G² = 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

r² =|G(X−C)|² = (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) = r². 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) = r². Expanding this, we obtain the quadratic equation for α and β,

A^τGAα²+ 2A^τGBαβ+B^τGBβ²+ 2A^τGEα+ 2B^τGEβ+ (E^τGE−r²) = 0.

(31)

The general quadratic equation for ellipse is

a₁₁α² +a₁₂αβ+a₂₂β²+a₁α+a₂β+a₀ = 0. (18)

In addition, we can use the property of ellipse thata₁₁a₂₂−^a₄²¹² >0. Hence,a₁₁>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

a₁₂= 2(A^τGB) = 2(A^τ(I−DD^τ)B) = 2(A^τB−(A^τD)(D^τB)) = 0.

If a₁₂= 0, then we can transform equation (18) into the form:

(α−α0

L_α )²+ (β−β0

L_β )² = 1,

where an ellipse center is (α₀, β₀)and axis half-lengths are L_α and L_β. We can get the values α₀ = _2a^−a¹

11, β₀ = _2a^−a²

22, L_α = √

a₂₂λ and L_β = √

a₁₁λ, where λ= ^a²¹^/(4a¹¹^)+a_a ²²^/(4a²²^)−a⁰

11a22 .

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=a₂₂/a₁₁ ∝p

a₂₂/a₁₁ =L_α/L_β. Hence, our observation is as follows:

~ yk =





 α₀ β0

u





 .

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:

(32)

J =







∂α0

∂c1

∂α0

∂c2

∂α0

∂c3

∂α0

∂x

∂α0

∂y

∂α0

∂z

∂α0

∂w

∂β0

∂c1

∂β0

∂c2

∂β0

∂c3

∂β0

∂x

∂β0

∂y

∂β0

∂z

∂β0

∂w

∂u

∂c1

∂u

∂c2

∂u

∂c3

∂u

∂x

∂u

∂y

∂u

∂z

∂u

∂w







=







∂(_2a^−a¹

11)

∂c1

∂(_2a^−a¹

11)

∂c2

∂(_2a^−a¹

11)

∂c3

∂(_2a^−a¹

11)

∂x

∂(_2a^−a¹

11)

∂y

∂(_2a^−a¹

11)

∂z

∂(_2a^−a¹

11)

∂w

∂(_2a^−a²

22)

∂c1

∂(_2a^−a²

22)

∂c2

∂(_2a^−a²

22)

∂c3

∂(_2a^−a²

22)

∂x

∂(_2a^−a²

22)

∂y

∂(_2a^−a²

22)

∂z

∂(_2a^−a²

22)

∂w

∂(^a_a²²

11)

∂c1

∂(^a_a²²

11)

∂c2

∂(^a_a²²

11)

∂c3

∂(^a_a²²

11)

∂x

∂(^a_a²²

11)

∂y

∂(^a_a²²

11)

∂z

∂(^a_a²²

11)

∂w





 .

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=







1−2y²−2z² 2xy−2zw 2xz+ 2yw 2xy+ 2zw 1−2x²−2z² 2yz−2xw 2xz−2yw 2yz+ 2xw 1−2x²−2y²





 .

Initially, the log has directionD=h

0 0 1 iτ

. At timet we can calculate direction of the cylinder as following:

D_t=P D =







2wy+ 2xz 2yz−2wx 1−2x²−2y²





 .

As we said, X-rays can be represented as a plane. This plane has normal vectorN = h

0 0 1 iτ

and point U = h

0 0 h1

iτ

. Therefore, we can denote A = h

1 0 0 iτ

and B =h

0 1 0 iτ

. 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

c₁ c₂ c₃ iτ

. Now we can estimate the coecients of the equation of ellipse as follows:

a₁₁=1−(2wy+ 2xz)², a₂₂=1−(2yz+ 2wx)²,

a₁ =(8(wy+xz)²−2)c1 + 8(wy+xz)(yz−wx)c2−

−4(wy+xz)(1−2x² −2y²)(h1−c3),

a₂ =8(wy+xz)(yz−wx)c1 + (8(yz −wx)²−2)c2−

−4(yz−wx)(1−2x²−2y²)(h1−c3).

(33)

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

0 0 h₁ iτ

and has normal vector N =h

0 0 1 iτ

. We can dene cylinder in parametric form as

x=rcos(t), y=rsin(t), z =z.

(19)

where z belongs to [−h, h] and 0≤ t ≤ 2π. Let us assume that v is a point of the 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:

P₃₁x+P₃₂y+P₃₃z+D₃−h₁ = 0⇒z = (h₁−D₃−P₃₁x−P₃₂y)/P₃₃. (21) Next, we substitute (19) into (21):

z = (h₁−D₃−P₃₁rcos(t)−P₃₂rsin(t))/P₃₃,

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 a₀ is not equal to zero, then we arbitrary assign a₀ = 1. This gives

a₁₁α²+a₁₂αβ+a₂₂β²+a₁α+a₂β+ 1 = 0.

(34)

5 OBSERVATION MODEL 34 By selecting random points (α_i, β_i) on the edge of ellipse we induce a system of linear equations, where a₁₁, a₁₂, a₂₂, a₁, a₂ are unknowns:

a₁₁α²_i +a₁₂α_iβ_i+a₂₂β_i²+a₁α_i+a₂β_i =−1.

In addition, we previously assumed that a₁₂ is equal to 0, then we derive

a₁₁α²_i +a₂₂β_i² +a₁α_i+a₂β_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, reconstruction 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 perform 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 operation 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.

(35)

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 extended 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.

(36)

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

Rigid body dynamics and Kalman filtering

Contents

1 INTRODUCTION

2 Background

2.1 Sawmill industry

3 Kalman lter

3.1 Basic Kalman lter

3.2 Extended Kalman lter

4 State transition model

4.1 Rigid body dynamics and physics engines

4.2 NVidia PhysX

4.3 A model of the motion of the log

4.4 Evolutionary operator

5 Observation model

5.1 Computed tomography

5.2 Lambert-Beer's law

5.3 Algebraic reconstruction method

5.4 A model of a X-ray scanner

5.5 Observation operator

6 Implementation

6.1 General overview

6.2 Numerical experiments