- About this Journal ·
- Abstracting and Indexing ·
- Aims and Scope ·
- Annual Issues ·
- Article Processing Charges ·
- Author Guidelines ·
- Citations to this Journal ·
- Contact Information ·
- Editorial Board ·
- Editorial Workflow ·
- Free eTOC Alerts ·
- Publication Ethics ·
- Reviewers Acknowledgment ·
- Submit a Manuscript ·
- Table of Contents

Advances in Mechanical Engineering

Volume 2013 (2013), Article ID 367127, 11 pages

http://dx.doi.org/10.1155/2013/367127

## Path Tracking Control of a Mobile Robot by Using Dual Estimation Algorithm

^{1}School of Mechanical Engineering, University of Ulsan, Ulsan 680-749, Republic of Korea^{2}Korea Aerospace Research Institute, Daejeon 305-806, Republic of Korea

Received 12 March 2013; Accepted 7 October 2013

Academic Editor: Hongxing Wei

Copyright © 2013 Xuan Vinh Ha et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

Skid-steered mobile robots have been developed to explore unknown environments, especially in rough terrain situations, where the wheel slips always occur and vary when the robot is traveling with different trajectory shapes. The wheel slippage limits the traction and braking abilities of the robot. In this paper, we propose a new dual estimation algorithm to overcome the previous limitations. The estimated values such as the robot’s positions and wheel slips are obtained based on the Kalman filtering technique. As demonstrated by our experimental results, the advantages of the new method are effective to overcome the slip limitations in the path tracking control problem of a four-track wheel skid-steered mobile robot (4-TW SSMR).

#### 1. Introduction

Recently, several studies have proposed and developed various approaches for estimating the position of a mobile robot. For example, one study used an extended Kalman filter (EKF) with a low-cost GPS unit and inclinometer to develop a localization scheme for Ackerman steering vehicles in indoor autonomous navigation situations by estimating the positions of the vehicles and their sensor biases [1]. In another study, a relative localization method was used to determine the navigational route of a convoy of robotic units in an indoor environment using an EKF based on a low-cost laser range system and built-in odometric sensors [2]. In skid-steered vehicles, discrepancies remain between position measurements and the navigational estimations obtained from odometry, infrared, and ultrasonic measurements and Kalman filtering techniques [3]. However, this study only mentioned the advantages of the different extensions of the KF for nonlinear estimations. In [4], a laser range finder (LRF) was used to find the localization of a mobile robot based on the EKF design. That study focused on a statistically exact derivation of the input noise covariance matrix, according to the known noise variances of the angular velocity measurements of both robot wheels.

Additionally, the exploration of unknown planetary surfaces with the help of mobile robot is increasingly being developed. A localization and path selection of any vehicle like a mobile robot to achieve any planetary exploration missions were described in [5]. In this study, by using odometry and gyro measurements, these sensors information was integrated by EKF. In another research, a robot, named Hyperion, was designed for solar-powered operation in polar environments and developed sun-cognizant navigation method to enable rovers to dodge shadows, seek sun, and drive sun-synchronous routes [6].

In a mobile robot’s field of operation, wheel slip limits traction and braking ability, especially for a 4-TW SSMR. Accurate estimation of slip is essential to obtain precise position for a mobile robot operating in unstructured terrain, especially in rough terrain situations. Several approaches to slip estimation have been developed. For example, a sliding mode observer (SMO) conducted to estimate slip parameters based on the kinematic model of a skid-steering vehicle is proposed in [7]. In another study, a novel method combining an optical flow algorithm with a sliding mode observer to estimate slip parameters of a SSMR was introduced [8]. Reference [9] proposed an experimental slip model for exact kinematic modeling, and the parameters of this model were determined based on experimental analysis. In [10], a rough-terrain control (RTC) methodology was presented in multi-wheeled mobile robot systems to improve ground traction and reduce power consumption. A key element of the method is able to estimate the wheel ground contact angles.

The methods in [7–9] remain limitations when the robot is traveling on the different ground surfaces with different and longer trajectory shapes. In this paper, we propose a new dual estimation algorithm for the robot’s position and wheel slip estimations based on the Kalman filtering technique. First, the EKF is utilized to estimate the robot’s positions, velocities and orientation angles, which are used for the feedback control signals. Second, a discrete KF is designed to estimate the slip parameters of the left and right track wheels, which are used to compensate for the velocity constraints and its variance values for the EKF in order to get more accurate estimated values. In the dual estimation algorithm, these two Kalman filters are performed simultaneously based on the measurements from AHRS sensor and two incremental encoders. The experimental results show that the advantages of the new method overcome the slip limitations in the path tracking control problem of a 4-TW SSMR. The experiment is performed on the NT-Hazard Escape-1, as shown in Figure 1.

This paper is organized as follows: Section 2 introduces the experimental setup and implemented control system. Dual estimation algorithm is presented in Section 3. The experimental results, including analysis and evaluation, are discussed in Section 4. Finally, Section 5 presents the conclusion.

#### 2. The Experimental Setup and Implemented Control System

##### 2.1. The Experimental Setup

The NT-Hazard Escape-1 mobile robot, with four NT-track wheels, considered in this study is applied in the investigation of disaster areas, running and watching rough areas including construction site and running up stairways. The robot with small actuators was made for the first time in Korea, providing various functions to robots. Each of the eight motors is faithful to its own move, and the robot can easily lift two tracks even when is fully loaded. The robot can also move while maintaining its central balance, like a tank. In addition, RS232C is supported to enable working with an embedded board and computer through wireless telecommunication. Therefore, it is easily applied in various applications. The dimensions for the mobile robot are shown in Figure 2.

For research purposes, an experimental mobile robot was designed. The system configuration was represented by a configuration for all of the elemental components as shown in Figure 3(a), while the real photograph of the experimental system is displayed in Figure 3(b). From these figures, the AHRS (AHRS-03-300) was mounted on the top and center of the robot based on a base platform. Two incremental encoders (E40H-8-1024-3-N-5) were installed on the robot’s front-left and right tracked wheels. A notebook (model ASUS U36S) was used to collect the data from the AHRS sensor and two incremental encoders via an AVR 128-pro board. This notebook also performed all algorithms which were applied in real-time. Here, the control algorithm for the system was coded in C# programming language (using the Microsoft Visual Studio 2008 version).

During the experiment, the measurement sampling frequency for the AHRS and incremental encoders is set to 20 Hz. The robot is programmed to follow a desired trajectory. Based on the measurements from these two sensors, the robot position values are derived through the Kalman filtering technique, and then robot controllers are applied to drive the robot to the desired trajectories.

##### 2.2. The Implemented Control System

In the experimental mobile robot, a path tracking control method for the robot motion is performed to follow the desired trajectory based on closed-loop control systems and the sensor fusion technique for feedback control signals. Figure 4 describes the diagram of the implemented control algorithm. As shown in this figure, the control system includes three closed-loop controls: two low-level closed-loop controls for motor speeds of the left and right sides and one high-level closed-loop control for robot position. The discrete PID controllers are applied for low-level closed-loop controls. For the path tracking control system, the stable tracking control (STC) method is introduced in [11, 12], based on an error model of the kinematical model. In another approach, vector field orientation (VFO) feedback control method is a motivating control technique to calculate such linear and angular velocities for DDVs. The reader can refer to [13] (an application for a Four wheel drive skid-steered mobile robot), [14] (VFO for a DDV) and [15] (an extension of VFO in the case of skid-slip phenomena). In this work, the VFO control feedback method is applied for the high-level closed-loop control of the robot position in a 2D plane.

Additionally, the dual estimation algorithm is proposed by integrating the EKF and the discrete KF. First, the EKF, denoted by EKF, is utilized to estimate the positions, velocities and orientation angles which are used for the feedback control signals in the high-level closed-loop control. Second, a discrete KF, denoted by KF, is designed to estimate the slip parameters of the left and right track wheels, which are used to compensate the velocity constraints and its variance values for the EKF in order to get more accurate estimation values. In the dual estimation algorithm, these EKF and KF are performed simultaneously based on the measurements from AHRS sensor and two incremental encoders, and they are introduced in the next sections.

#### 3. Dual Estimation Algorithm

##### 3.1. AHRS Kinematics and Velocity Constraints

We define a navigational reference frame and robot body frame as shown in Figure 5. Let , , and denote the position, velocity, and attitude angle vectors of AHRS in frame, respectively. We also define the AHRS acceleration, angular rate, acceleration walking bias, and angular rate walking bias in the frame as , , , and , respectively.

After subtracting the constant offset and local gravity vector, the acceleration and angular rate models of the AHRS can be described by [16]

with

where the true acceleration vector, true angular rate vector, acceleration white noise, and angular rate white noise are , , , and , respectively. And , , , , and are time constants, noise variances, and zero-mean white noises with of the acceleration walking bias and angular rate walking bias, and the is the sampling frequency.

We also define the state vector of model process. The kinematic motion equation for AHRS can be simplified to:

where and , the transformation matrix from the frame to the frame and transformation matrix of the Euler angles, as given by the following matrices:

where , , , and the same notation convention is used for angles and .

In the 4-TW SSMR, the motors that power the wheels at each side are geared internally to ensure that the velocities of two adjacent wheels at each side are synchronized (have the same angular velocity) and thus have the same velocity at ground contact. Let us define and as the left and right linear velocities of the robot; we have where , , , and are center linear velocities for front-left, rear-left, front-right, and rear-right wheels, respectively, shown in Figure 6. The longitudinal wheel slips of the left and right wheels and are defined as ratios of the wheel velocities and its center velocities [17], as follows: where is the wheel radius, and are the wheel angular speeds for the left and right sides of the mobile robot. From (7), it can be seen that the wheel slip if the wheel is under traction and if the wheel is under braking. Using the definition of the slip in (7), we have

Because of the symmetric mechanical structure of the robot, it can be assumed that the center of mass (COM) of the robot is located at the center of geometry (COG) of the body frame. The AHRS coordinate is located at the COG in frame, as shown in Figure 5. Using two wheel encoders, we obtain the AHRS velocity vector in frame. From (8), using two wheel encoder measurements and wheel slip parameters, we obtain the longitudinal velocity as follows:

Based on [17], since the four tracked wheels of our robot are always in contact with the ground and since the AHRS is fixed on the robot platform, the velocity constraints and in -axis and -axis directions for the AHRS device can be simplified to equal zero.

The noises in the longitudinal velocity can be expressed as the sum of the noises of the left and right wheels’ angular speeds and as measured by encoders. It is assumed that the noises for and have a normal distribution with a zero-mean and corresponding variances, and there is no cross-correlation between the noise of and of [18]. The noise variance of the longitudinal velocity can be expressed as

The noise variances and of the lateral velocity and ground surface topography are also expressed by zeros. In order to obtain the longitudinal velocity and its noise variance, we have to estimate slips and . The proposed dual estimation method, which can estimate these two slip values, is introduced in Section 3.2.

##### 3.2. Dual Estimation Algorithm

Now, we define the state variable’s vector . We rewrite the AHRS kinematics equation shown in (3) in discrete-time form, and we obtain the process model as

where the AHRS input signals at the th sampling time is and the data sampling period is . Also, the nonlinear function of the process model is

and is the process noise vector.

With velocity constraints (9), the AHRS velocities in frame are considered as the measurement vector . Including the wheel encoder measurement noise and ground topography, the measurement model is rewritten in discrete-time form as

where is measurement noises. We assume that the measurement noises are independent, zero-mean, and Gaussian white noise; that is, . The covariance matrix of the measurement noise is

where , , and are calculated in (10). The EKF was implemented by using systems (11) and (13) in order to obtain the estimated positions, velocities, and attitudes of the robot.

Additionally, in order to determine wheel slip parameters to compensate the velocity constraints and its variance values, a discrete KF, denoted by KF, is designed. By giving in body frame and yaw rate , the wheel center velocities along -axis for the left and right sides wheels are calculated by

where is lateral wheel bases. By substituting (15) into (7), we have

We define unknown parameters as:

Substituting (18) into (17), the relationship between the angular speed from encoders and unknown parameters is described as

The slip estimation based on the KF is now established. Assuming that the slip process is the random process, then the unknown parameter process can be modeled in discrete-time form as

where is the process state vector of the KF at time step, is process noise vector, and is the state transition matrix which is defined as:

From (19), the measurement model of the KF can be written relating to the encoder measurement noises as

where is angular speed vector from two encoders at time step, is measurement noise vector, and is the measurement matrix which is calculated as follows:

where and are the estimated values of the linear and angular velocities of mobile robot in frame which are computed as

where , , and are given from the EKF. The Systems (20) and (22) are used to perform the KF in order to estimate the unknown parameters of the left and right sides. From (18), the estimated slip parameters and at step are calculated through the estimated unknown parameters and as follows:

From previous descriptions, the estimated slip values and are obtained indirectly through the unknown parameters and . In this method, the current values and (in (24)) are calculated by using the current state and of the EKF. Similarly, the current estimated slip values in (25) are used to compute the longitudinal velocity in (9) and its noise variance value in (10) in order to perform the EKF. From the previous analyses, in this work, there are two Kalman filters running concurrently; it is called *the dual estimation algorithm* [19, 20]. As illustrated in Figure 7, the algorithm consists of two kinds of Kalman filters that run concurrently. The top EKF generates state estimates and requires for the measurement update. The bottom KF generates slip state estimates and requires for the measurement update. The following describes the dual estimation process through mathematical equations [21, 22].

Initialize with

For , the prediction step for KF is

and update step equations for KF are

where and are the process and measurement covariance matrices of and , respectively.

And the prediction step for EKF is

and update step for EKF is

where

and is the process covariance matrix of .

Update estimation values and for KF as

#### 4. Experimental Results

In this section, the experiments are performed to prove the effectiveness of the proposed dual estimation algorithm (DEA) for the robot’s path tracking problem in real-time application. The VFO feedback control method is applied for the trajectory tracking control. In fact, the wheel slips, which always occur and vary while the robot is traveling on the different ground surfaces with different trajectory shapes, limit the traction and braking abilities of the robot, especially in 4-TW SSMR. Therefore, a proposed DEA is performed to overcome the previous limitations in this research. The estimated values such as robot’s positions, velocities, attitudes and wheel slip parameters are obtained more accurately, which are used for the feedback control signals in the high-level closed-loop control. In this work, the experiments have been carried out on concrete terrain in an outdoor environment, involving two scenarios.

*Scenario 1*. The mobile robot is controlled to follow a desired trajectory with two curves.

*Scenario 2*. The mobile robot is controlled to follow a desired circular trajectory with a 2 m radius.

In the first scenario, the output trajectories are shown in Figure 9(a). As illustrated in Figure 9(a), the output response without DEA is quite far to the desired response while that with DEA is close to the desired response. The linear and angular velocities of two methods (without and with DEA), shown in Figures 8(a) and 8(b), are also close to the desired velocities despite the mechanical vibrations and sensor noises. In fact, although the working environment contains large numbers of nonlinear and uncertain factors, which impact with the system performances, the results point out that the proposed DEA has improved the performance of the path tracking problem. The previous evaluation is proved by considering the position errors as shown in Figure 9(b). In Figure 9(b), the position error with DEA at any instant of time, which is the distance from the estimated position of the robot at the defined instant to the nearest point of the desired trajectory, is the closest to zero compared to that without DEA. Consequently, the root mean square error (RMSE) of robot position with DEA is the smallest (0.0239 [m]), while that without DEA is 0.0845 [m].

In order to evaluate the proposed method, the system is also tested with a circular trajectory, in the second scenario. The output responses without and with DEA are shown in Figure 11(a). It can see that the response with DEA is still the closest to the desired response. The linear and angular velocities are shown in Figures 10(a) and 10(b). Figure 11(b) clearly depicts that the result with DEA is the best while comparing to that without DEA; the position error graph with DEA is still the closest to zero. Hence, the RMSE of robot position with DEA is the smallest (0.0236 [m]), while that without DEA is 0.0882 [m].

In both scenarios, the results show that the trajectories with DEA are the closest to the desired responses. It results from the estimated slip values which are used to compensate the velocity constraints and its variances. The estimated values in EKF such as velocities and trajectories with DEA are more precise than those without DEA. It means that the feedback control signals with DEA are more accurate, and then the high-level closed-loop control with DEA performs better than the previous testbed without DEA.

From the previous analyses, the robot positioning results with DEA are better than the previous results without DEA. It is clear that the DEA, with wheel slip compensation, is very essential in the mobile robot field. For the first scenario, the robot follows the desired trajectory, which has two curves. At the left rotation in the first curve, the angular speed of the right wheels is greater than that of the left wheels. Then, the traction phenomenon occurs on the right side, while the braking phenomenon appears on the left. Consequently, the slip value of the right wheels is positive, and that of the left wheels is negative. Similarly, at the right rotation in the second curve, the slip value of the right wheels is negative while that of the left side is positive. Furthermore, when the robot travels along the straight segment in the first trajectory, less force is needed to drive the robot and the left and right wheel slips are close to zero. Figure 12(a) illustrates the estimated slip parameters of the left and right wheels in the first scenario.

For the second scenario, the robot follows the desired circular trajectory with counterclockwise direction. The angular speed of the right wheels is always larger than that of the left wheels. Consequently, the slip value of the right wheels is always positive and that of the left wheels is always negative, as shown in Figure 12(b). All types of the estimated slip values in both scenarios are consistent. On the whole, the robot positioning results are improved significantly by considering the new dual estimation method. Table 1 shows RMSEs of all experiments in this work; the RMSE values with DEA are reduced more significantly.

#### 5. Conclusion

This paper presents experiments to control the 4-TW SSMR based on the estimated feedback control signals. In the mobile robot field, the wheel slip limits the traction and braking ability of the robot, especially the 4-TW SSMR. The wheel slip always occurs while the robot is traveling on the different ground surfaces with different trajectory shapes. In this paper, we propose the new dual estimation algorithm for robot’s positions, velocities, attitudes, and wheel slip parameters based on the Kalman filtering technique in order to obtain more accurate estimation values, which are used for the feedback control signals. In the dual estimation algorithm, two Kalman filters performed simultaneously based on the measurements from AHRS sensor and two incremental encoders. Experimental results show the advantages of the novel method which is effective in the path tracking control problem for a 4-TW SSMR, for example, the commercialized Hazard Escape I mobile robot.

#### Conflict of Interests

This company does not have any relationship with the name of Hazard Escape 1. So there is no competing interest such as financial gain between two sides.

#### Acknowledgment

This research was supported by Korea Aerospace Research Institute in 2012.

#### References

- A. J. Weinstein and K. L. Moore, “Pose estimation of Ackerman steering vehicles for outdoors autonomous navigation,” in
*Proceedings of the IEEE International Conference on Industrial Technology (ICIT '10)*, pp. 579–584, Viña del Mar Valparaíso, Chile, March 2010. View at Publisher · View at Google Scholar · View at Scopus - F. Espinosa, C. Santos, M. Marrón-Romera, D. Pizarro, F. Valdés, and J. Dongil, “Odometry and laser scanner fusion based on a discrete extended Kalman filter for robotic platooning guidance,”
*Sensors*, vol. 11, no. 9, pp. 8339–8357, 2011. View at Publisher · View at Google Scholar · View at Scopus - L. Tamás, G. Lazea, R. Robotin, C. Marcu, S. Herle, and Z. Szekely, “State estimation based on Kalman filtering techniques in navigation,” in
*Proceedings of the IEEE International Conference on Automation, Quality and Testing, Robotics (AQTR '08)*, vol. 2, pp. 147–152, Cluj-Napoca, Romania, May 2008. View at Publisher · View at Google Scholar · View at Scopus - L. Teslić, G. Klančar, and I. Škrjanc, “Kalman-filtering-based localization of a mobile robot with a LRF in a simulated 2D environment,” in
*Proceedings of the IEEE Mediterranean Electrotechnical Conference (MELECON '08)*, pp. 316–322, Ajaccio, France, May 2008. View at Publisher · View at Google Scholar · View at Scopus - L. Yenilmez and H. Temeltas, “Autonomous navigation for planetary exploration by a mobile robot,” in
*Proceedings of the International Conference on Recent Advances in Space Technologies*, pp. 397–402, November 2003. - D. Wettergreen, B. Shamah, P. Paul Tompkins, and W. Whittaker, “Robotic planetary exploration by sun-synchronous navigation,” in
*Proceedings of the 6th International Symposium on Artificial Intelligence, Robotics and Automation in Space (i-SAIRAS '01)*, June 2001. - S. Zibin, Y. H. Zweiri, L. D. Seneviratne, and K. Althoefer, “Non-linear observer for slip estimation of skid-steering vehicles,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '06)*, pp. 1499–1504, Orlando, Fla, USA, May 2006. View at Publisher · View at Google Scholar · View at Scopus - X. Song, L. D. Seneviratne, K. Althoefer, and Z. Song, “A robust slip estimation method for skid-steered mobile robots,” in
*Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV '08)*, pp. 279–284, Hanoi Hoan Kiem District, Vietnam, December 2008. View at Publisher · View at Google Scholar · View at Scopus - S. A. A. Moosavian and A. Kalantari, “Experimental slip estimation for exact kinematics modeling and control of a Tracked Mobile Robot,” in
*Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '08)*, pp. 95–100, Nice, France, September 2008. View at Publisher · View at Google Scholar · View at Scopus - K. Iagnemma and S. Dubowsky, “Mobile Robot Rough-Terrain Control (RTC) for planetary exploration,” in
*Proceedings of the 26th ASME Biennial Mechanisms and Robotics Conference (DETC '00)*, 2000. - Y. Kanayama, Y. Kimura, F. Miyazaki, and T. Noguchi, “A stable tracking control method for an autonomous mobile robot,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, vol. 1, pp. 384–389, May 1990. View at Scopus - E. Maalouf, M. Saad, and H. Saliah, “A higher level path tracking controller for a four-wheel differentially steered mobile robot,”
*Robotics and Autonomous Systems*, vol. 54, no. 1, pp. 23–33, 2006. View at Publisher · View at Google Scholar · View at Scopus - S. Arslan and H. Temeltaş, “Robust motion control of a four wheel drive skid-steered mobile robot,” in
*Proceedings of the 7th International Conference on Electrical and Electronics Engineering (ELECO '11)*, pp. II415–II419, December 2011. View at Scopus - M. Michałek and K. Kozłowski, “Vector-field-orientation feedback control method for a differentially driven vehicle,”
*IEEE Transactions on Control Systems Technology*, vol. 18, no. 1, pp. 45–65, 2010. View at Publisher · View at Google Scholar · View at Scopus - M. M. Michałek, P. Dutkiewicz, M. Kiełczewski, and D. Pazderski, “Vector-field-orientation tracking control for a mobile vehicle disturbed by the skid-slip phenomena,”
*Journal of Intelligent and Robotic Systems*, vol. 59, no. 3-4, pp. 341–365, 2010. View at Publisher · View at Google Scholar · View at Scopus - W. S. Flenniken IV, J. H. Wall, and D. M. Bevly, “Characterization of various IMU error sources and the effect on navigation performance,” in
*Proceedings of the 18th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS '05)*, pp. 967–978, September 2005. View at Scopus - J. Yi, H. Wang, J. Zhang, D. Song, S. Jayasuriya, and J. Liu, “Kinematic modeling and analysis of skid-steered mobile robots with applications to low-cost inertial-measurement-unit-based motion estimation,”
*IEEE Transactions on Robotics*, vol. 25, no. 5, pp. 1087–1097, 2009. View at Publisher · View at Google Scholar · View at Scopus - L. Teslić, G. Klančar, and I. Škrjanc, “Kalman-filtering-based localization of a mobile robot with a LRF in a simulated 2D environment,” in
*Proceedings of the IEEE Mediterranean Electrotechnical Conference (MELECON '08)*, pp. 316–322, Ajaccio, France, May 2008. View at Publisher · View at Google Scholar · View at Scopus - E. A. Wan, R. V. D. Merwe, and A. T. Nelson, “Dual estimation and the unscented transformation,” in
*Advances in Neural Information Processing Systems*, vol. 12, pp. 666–672, November 2000. - S. Haykin,
*Kalman Filtering and Neural Networks*, Jokn Wiley & Sons, 1st edition, 2001. - G. Welch and G. Bishop,
*An Introduction to the Kalman Filter*, University of North Carolina at Chapel Hill, 1995. - R. G. Brown and P. Y. C. Hwang,
*Introduction to Random Signals and Applied Kalman Filtering*, 3rd edition, 1997.