Introduction to kalman filter pdf

The TRN algorithm blends a navigational solution from an inertial navigation system INS with the measured terrain profile underneath the aircraft.

Terrain measurements have generally been obtained by using radar altimeters. TRN systems using cameras [ 7 ], airborne laser sensors [ 8 ], and interferometric radar altimeters [ 9 ] have also been addressed.

The simple model in 33 is considered realistic without details of INS integration if an independent attitude solution is available so that the velocity can be resolved in an earth-fixed frame [ 10 ]. The estimation models we deal with belong to the TRN filter block in Figure 6 , taking relative movement information from the INS as u k.

Conventional TRN structure. Typical TRN systems utilize measurements of the terrain elevation underneath an aircraft. The terrain elevation measurement is modeled as:. The elevation measurement is obtained by subtracting the ground clearance measurement from a radar altimeter, h r , from the barometric altimeter measurement, h b.

The ground clearance and the barometric altitude correspond to the above ground level AGL height and the mean sea level MSL height, respectively. The relationship between the measurements is depicted in Figure 7. Note that the terrain elevation that comprises the measurement model in 34 is highly nonlinear. Relationship between measurements in TRN. The process model in 33 and the measurement model in 34 can be linearized as:. The DEMs are essentially provided as matrices containing grid-spaced elevation data.

For obtaining finer-resolution data, interpolation techniques are often used to estimate the unknown value in between the grid points. One of the simplest methods is linear interpolation. Linear interpolation is quick and easy, but it is not very precise. A generalization of linear interpolation is polynomial interpolation. Polynomial interpolation expresses data points as higher degree polynomial. Polynomial interpolation overcomes most of the problems of linear interpolation. However, calculating the interpolating polynomial is computationally expensive.

Furthermore, the shape of the resulting curve may be different to what is known about the data, especially for very high or low values of the independent variable.

These disadvantages can be resolved by using spline interpolation. Spline interpolation uses low-degree polynomials in each of the data intervals and let the polynomial pieces fit smoothly together. That is, its second derivative is zero at the grid points see [ 11 ] for more details. Classical approach to use polynomials of degree 3 is called cubic spline.

Because the elevation data are contained in a two-dimensional array, bilinear or bicubic interpolation are generally used.

Cubic spline interpolation is used in this example. The profile of the DEM can be depicted as Figure 8. The figure represents contours of the terrain where brighter color denotes regions with higher altitude. The point 20, 10 in the grid corresponds to the position T in the navigation frame. Contour representation of terrain profile.

The aircraft is equipped with a radar altimter and a barometric altimter, which are used for obtaining the terrain elevation. The radar altimeter is corrupted with a zero-mean Gaussian noise with the standard deviation of 3.

The matrices Q and R are following the real statistics of the noises as:. The above equation means the error of the initial guess for the target state is randomly sampled from a Gaussian distribution with a standard deviation of 50 One can observe the RMSE converges relatively slower than other examples.

Because the TRN estimates 2D position by using the height measurements, it often lacks information on the vehicle state.

Moreover, note that the extended Kalman filter linearizes the terrain model and deals with the slope that is effective locally. If the gradient of the terrain is zero, the measurement matrix H has zero-diagonal terms that has zero effect on the state correction. In this case, the measurement is called ambiguous [ 12 ] and this ambiguous measurement often causes filter degradation and divergence even in nonlinear filtering techniques.

With highly nonlinear terrain models, TRN systems have recently been constructed with other nonlinear filtering methods such as point-mass filters and particle filters, rather than extended Kalman filters. Time history of RMSE. In this chapter, we introduced the Kalman filter and extended Kalman filter algorithms.

This chapter will become a prerequisite for other contents in the book for those who do not have a strong background in estimation theory. Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution 3. Help us write another book on this subject and reach those readers.

Login to your personal dashboard for more detailed statistics on your publications. Edited by Felix Govaers. Edited by Dumitru Baleanu. We are IntechOpen, the world's leading publisher of Open Access books. Built by scientists, for scientists. Our readership spans scientists, professors, researchers, librarians, and students, as well as business professionals. Downloaded: Abstract We provide a tutorial-like description of Kalman filter and extended Kalman filter. On this same plot, a white noise would merely extend this constant power level out across all frequencies.

Now, within the bandpass of the system of interest, the fictitious white noise looks identical to the real wideband noise. So what has been gained? That is the second part of the answer to why a white noise model is used. Therefore, the white noise model is used. One might argue that there are cases in which the noise power level is not constant over all frequencies within the system bandpass, or in which the noise is in fact time correlated.

For such instances, a white noise put through a small linear system can duplicate virtually any form of time-correlated noise. Whereas whiteness pertains to time or frequency relationships of a noise, Gaussianness has to do with its amplitude.

Thus, at any single point in time, the probability density of a Gaussian noise amplitude takes on the shape of a normal bell-shaped curve. This assumption can be justified physically by the fact that a system or measurement noise is typically caused by a number of small sources.

It can be shown mathematically that when a number of inde- pendent random variables are added together, the summed effect can be de- scribed very closely by a Gaussian probability density, regardless of the shape of the individual densities. Similar to whiteness, it makes the mathematics tractable. But more than that, typically an engineer will know, at best, the first and second order statistics mean and variance or standard deviation of a noise process.

In the absence of any higher order statistics, there is no better form to assume than the Gaussian density. The first and second order statistics completely determine a Gaussian density, unlike most densities which require an endless number of orders of statistics to specify their shape entirely. Thus, the Kalman filter, which propa- gates the first and second order statistics, includes all information contained in the conditional probability density, rather than only some of it, as would be the case with a different form of density.

The particular assumptions that are made are dictated by the objectives of, and the underlying motivation for, the model being developed. If our objective were merely to build good descriptive models, we would not confine our atten- tion to linear system models driven by white Gaussian noise.

For- tunately, the class of models that yields tractable mathematics also provides adequate representations for many applications of interest. Later, the model structure will be extended somewhat to enlarge the range of applicability, but the requirement of model usefulness in subsequent estimator or controller design will again be a dominant influence on the manner in which the exten- sions are made.

Suppose that you are lost at sea during the night and have no idea at all of your location. So you take a star sighting to establish your position for the sake of simplicity, consider a one-dimensional location.

At some time t 1 you determine your location to be z 1. However, because of inherent measuring device inaccuracies, human error, and the like, the result of your measurement is somewhat uncertain. This is a plot of f x t 1 z t 1 x z 1 as a function of the location x : it tells you the probability of being in any one location, based upon the measurement you took.

For a Gaussian density, Because he has a higher skill, assume the variance in his measurement to be somewhat smaller than in yours. Note the narrower peak due to smaller variance, indi- cating that you are rather certain of your position based on his measurement. At this point, you have two measurements available for estimating your position. The question is, how do you combine these data?

It is the mode and the mean or, since it is the mean of a conditional density, it is also termed the conditional mean. Furthermore, it is also the maximum likelihood estimate, the weighted least squares estimate, and the linear estimate whose variance is less than that of any other linear unbiased estimate. Using the K t 2 in Eq. Stated differently, by propagating these two variables, the conditional density of your position at time t 2 , given z 1 and z 2 , is completely specified.

Thus we have solved the static estimation problem. Now consider incorpo- rating dynamics into the problem. Suppose that you travel for some time before taking another measurement. At time t 2 it is as previously derived. As time progresses, the density travels along the x axis at the nominal speed u , while simultaneously spreading out about its mean. Thus, the probability density starts at the best estimate, moves according to the nominal model of dynamics, f x t z t ,z t x z 1 ,z 2 1 2 FIG.

At the time t 3— , just before the measurement is taken at time t 3 , the density f x t 3 z t 1 ,z t 2 x z 1 ,z 2 is as shown in Fig. As before, there are now two Gaussian densities available that contain information about position, one encompassing all the information available before the measurement, and the other being the infor- mation provided by the measurement itself.

The best prediction of its value before z 3 is taken is corrected by an optimal weighting value times the difference between z 3 and the predic- tion of its value. Similarly, the variance and gain equations are of the same form as and Observe the form of the equation for K t 3.

Although we have not as yet derived these results mathematically, we have been able to demonstrate the reasonableness of the filter structure. Similarly, the Kalman filter update at a measurement time is just the extension of Eqs. Fur- ther logical extensions would include estimation with data beyond the time when variables are to be estimated, estimation with nonlinear system models rather than linear, control of systems described through stochastic models, and both estimation and control when the noise and system parameters are not known with absolute certainty.

The sequel provides a thorough investigation of those topics, developing both the theoretical mathematical aspects and practical engineering insights necessary to resolve the problem formulations and solutions fully.

Aoki, M. Academic Press, New York, Bryson, A. Blaisdell, Wahham, Massachusetts, Bucy, R. Wiley, New York, Deutsch, R. Deyst, J. Press, Cambridge, Massachusetts, Jazwinski, A. Lee, R. Press, Cambridge, Mas- sachusetts, Liebelt, P. Addison-Wesley, Reading, Massachusetts, Meditch, J. McGraw-Hill, New York, McGarty, T. Sage, A. Schweppe, F. Van Trees, H. The method, which is applicable to a wide variety of both general, and tracking for virtual environments in particular.

Most commercial and experimental systems, improves accuracy by of us have the preconceived notion that to estimate a set of properly assimilating sequential observations, filtering sensor unknowns we need as many constraints as there are degrees of measurements, and by concurrently autocalibrating source and freedom at any particular instant in time. What we present instead sensor devices.

It facilitates user motion prediction, multisensor is a method to constrain the unknowns over time , continually data fusion, and higher report rates with lower latency than refining an estimate for the solution, a single constraint at a time. For applications in which the constraints are provided by real- Tracking systems determine the user's pose by measuring time observations of physical devices, e.

For reasons of physics of sensors or visual sightings of landmarks, the SCAAT method and economics, most systems make multiple sequential isolates the effects of error in individual measurements. This measurements which are then combined to produce a single tracker isolation can provide improved filtering as well as the ability to report.

For example, commercial magnetic trackers using the individually calibrate the respective devices or landmarks SPASYN Space Synchro system sequentially measure three concurrently and continually while tracking. The method magnetic vectors and then combine them mathematically to facilitates user motion prediction, multisensor or multiple modality produce a report of the sensor pose. Because single observations With respect to tracking for virtual environments, we are under-constrain the mathematical solution, we refer to our currently using the SCAAT method with a new version of the UNC approach as single-constraint-at-a-time or SCAAT tracking.

The wide-area optoelectronic tracking system section 4. The method key is that the single observations provide some information about could also be used by developers of commercial tracking systems the user's state, and thus can be used to incrementally improve a to improve their existing systems or it could be employed by end- previous estimate. We recursively apply this principle, users to improve custom multiple modality hybrid systems.

With incorporating new sensor data as soon as it is measured. With this respect to the more general problem of estimating a set of approach we are able to generate estimates more frequently, with unknowns that are related by some set of mathematical constraints, less latency, and with improved accuracy. We present results from one could use the method to trade estimate quality for computation both an actual implementation, and from extensive simulations.

For example one could incorporate individual constraints, one at a time, stopping when the uncertainty in the solution CR Categories and Subject Descriptors: I. For example, for a camera observing landmarks in a latency, sensor fusion, Kalman filter. Given a particular system, and the corresponding set of unknowns that are to be estimated, let C be defined as the minimal number of independent simultaneous constraints necessary to uniquely determine a solution, let N be the number actually used to generate a new estimate, and let N ind be the number of independent constraints that can be formed from the N constraints.

See Figure 1. C is the minimal number of independent simultaneous constraints necessary to uniquely determine a solution, N is the number of given constraints, and N ind is the number of independent constraints that can be formed from the N. In this case, the constraints provided by the incomplete constraints would be characterized as locally observations are multi-dimensional: 2D image coordinates of 3D unobservable.

Such a system must incorporate a sufficient set of scene points. Given the internal camera parameters, a set of four these incomplete constraints so that the resulting overall system is known coplanar scene points, and the corresponding image observable.

The corresponding aggregate measurement model can coordinates, the camera position and orientation can be uniquely then be characterized as globally observable. Global observability determined in closed-form [16]. The SCAAT method constraints 2D image points are used to estimate the camera adopts the latter scheme, even in some cases where the former is position and orientation, the system is completely observable. On possible. Therefore, if the individual sequentially. For example, tracking systems developed by observations provide only partial information, i.

A system that devices or landmarks must be excited and or sensed prior to facilitated simultaneous polarized excitations would be very estimating a solution. Sometimes the necessary observations can difficult if not impossible to implement. Similarly both the original be obtained simultaneously, and sometimes they can not. Magnetic UNC optoelectronic tracking system and the newer HiBall version trackers such as those made by Polhemus and Ascension perform are designed to observe only one ceiling-mounted LED at a time.

And while a camera can indeed assume mathematically that their sequential observations were observe multiple landmarks simultaneously in a single image, the collected simultaneously. We refer to this as the simultaneity image processing to identify and locate the individual landmarks assumption. If the target remains motionless this assumption must be done sequentially for a single CPU system. If the introduces no error. However if the target is moving, the violation landmarks can move independently over time, for example if they of the assumption introduces error.

For the SCAAT implementation might grab an image, extract a single current versions of the above systems such motion corresponds to landmark, update the estimates of both the camera and landmark approximately 2 to 6 centimeters of translation throughout the positions, and then throw-away the image.

In this way estimates sequence of measurements required for a single estimate. For are generated faster and with the most recent landmark systems that attempt sub-millimeter accuracies, even slow motion configurations. The error introduced by violation of the simultaneity image deflection techniques are sometimes employed in an attempt assumption is of greatest concern perhaps when attempting any to address latency variability in the rendering pipeline [32,39]. Gottschalk and Hughes note that Such methods are most effective when they have access to or motion during their autocalibration procedure must be severely generate accurate motion predictions and low-latency tracker restricted in order to avoid such errors [19].

Consider that for a updates. With accurate prediction the best possible position and multiple-measurement system with 30 milliseconds total orientation information can be used to render a preliminary image. For has been detected and incorporated into the deflection. Let t m be the time needed to determine one constraint, e. While intrinsic sensor time needed to actually compute that estimate.

Then the estimate parameters can often be determined off-line, e. Similarly the precise geometric relationship between visible As the number of constraints N increases, equation 1 shows how landmarks used in a vision-based system is often difficult to the estimate latency and rate increase and decrease respectively. Because the SCAAT method isolates improve the latencies and data rates of such systems by updating the individual measurements, or measurement dimensions, the current estimate with each new individual constraint, i.

In other words, it increases the estimate rate to identified and dealt with. Furthermore, because the simultaneity approximately the rate that individual constraints can be obtained assumption is avoided, the motion restrictions discussed in and likewise decreases the estimate latency to approximately the section 2. The isolation enforced by the SCAAT approach can improve Figure 2 illustrates the increased data rate with a timing results even if the constraints are obtained simultaneously through diagram that compares the SPASYN Polhemus Navigation multidimensional measurements.

An intuitive explanation is that if Systems magnetic position and orientation tracking system with a the elements dimensions are corrupted by independent noise, hypothetical SCAAT implementation.

In contrast to the SPASYN then incorporating the elements independently can offer improved system, a SCAAT implementation would generate a new estimate filtering over a batch or ensemble estimation scheme. Given that common arm and head motion bandwidth specifications Sensor Measurement range from 2 to 20 Hz [13,14,36], the sampling rate should ideally be greater than 40 Hz. Furthermore, the estimate rate should be as SPASYN Estimate high as possible so that normally-distributed white estimate error can be discriminated from any non-white error that might be SCAAT Estimate observed during times of significant target dynamics, and so estimates will always reflect the most recent user motion.

In addition to increasing the estimate rate, we want to reduce time the latency associated with generating an improved estimate, thus reducing the overall latency between target motion and visual Figure 2: A timing diagram comparing the SPASYN feedback in virtual environment systems [34].

If too high, such Polhemus Navigation Systems magnetic position and latency can impair adaptation and the illusion of presence [22], and orientation tracking system with a hypothetical SCAAT can cause motion discomfort or sickness. Increased latency also implementation. The Kalman filter [26] has been widely used for data fusion. The vision-based component attempts to identify and 3. A SCAAT The use of a Kalman filter requires a mathematical state-space implementation would instead identify and locate only one model for the dynamics of the process to be estimated, the target landmark per update, using a new image frame each time.

Not motion in this case. While several possible dynamic models and only would this approach increase the frequency of landmark- associated state configurations are possible, we have found a based correction given the necessary image processing but it simple position-velocity model to suffice for the dynamics of our would offer the added benefit that unlike the implementation applications.

Discussion of some other potential models and number C necessary to determine a complete position and the associated trade-offs can be found in [7] pp. Because orientation solution. In the standard model corresponding to equation 2 , the n dimensional Kalman filter state vector x t would completely 3 METHOD describe the target position and orientation at any time t.

In practice we use a method similar to [2,6] and maintain the The SCAAT method employs a Kalman filter KF in an unusual complete target orientation externally to the Kalman filter in order fashion. The Kalman filter is a mathematical procedure that to avoid the nonlinearities associated with orientation provides an efficient computational recursive method for the computations. In the internal state vector x t we maintain the least-squares estimation of a linear system.

S e e [ 9 ] f o r measurement model. The extended Kalman filter EKF is a discussion of quaternions. Thus the introduction to the Kalman filter can be found in Chapter 1 of [31], incremental orientations are linearized for the EKF, centered about while a more complete introductory discussion can be found in zero. We maintain the derivatives of the target position and [40], which also contains some interesting historical narrative. We maintain the More extensive references can be found in [7,18,24,28,31,46].

For example see angles themselves. The target state is then represented by the [2,5,12,14,42], and most recently [32]. In section 3. Predict the state and error covariance. The measurement model is used to c. Predict the measurement and compute the corresponding Jaco- predict the ideal noise-free response of each sensor and source bian.

Compute the Kalman gain. However if f. Correct the predicted tracker state estimate and error covariance it is not possible or necessary to isolate the measurements, e. Zero the orientation elements of the state vector.

The method we use for autocalibration involves augmenting the main tracker filter presented in section 3. The computations include for example scene landmarks which can be thought of as can be optimized to eliminate operations on matrix and vector passive sources, and cameras which are indeed sensors.

In elements that are known to be zero. In addition, the matrix inverted in the autocalibration method. Finally, the to be calibrated since omission of one or the other is trivial. The total per estimate computation time can therefore actually be less than that of a corresponding conventional 3. For the 3D-tracking problem being solved, a unique c.

Because individual sensor measurements estimates from a above. After step a in the original algorithm, we form time t 1 …t N , and then proceed to compute an estimate. In any case, for such well-constrained the error covariance matrix systems complete observability is obtained at each step of the filter. Note that the and the process noise matrix inversion in 13 forms a ratio that reflects the relative uncertainties of the state and the measurement.

We then follow steps b - h from the original algorithm, making 3. A linear system is said to be stable if its response to any input tends to a finite steady value after vector x per After step h we finish by extracting and saving the device filter portions of the augmented state vector and the input is removed [24].

In general these conditions are main tracker filter, the source filter, and the sensor filter easily met for systems and circumstances that would otherwise be respectively. We leave the main tracker filter state vector and stable with a multiple-constraint implementation.

A complete error covariance matrix in their augmented counterparts, while we stability analysis is beyond the scope of this paper, and is presented swap the device filter components in and out with each estimate.

The result is that individual device filters are updated less frequently than the main tracker filter. The more a device is used, 3. If a device is never used, it is never Beyond a simple round-robin approach, one might envision a calibrated. In doing so one would like to be able to again be optimized to eliminate operations on matrix and vector monitor and control uncertainty in the state vector.

By periodically elements that are known to be zero: those places mentioned in observing the eigenvalues and eigenvectors of the error covariance section 3.

Also note that the size of and thus matrix P t , one can determine the directions in state-space along time for the matrix inversion in 13 has not changed. With respect which more or less information is needed [21]. Assuming 8 bytes per word, HiBall tracker. First, the dynamics of the two sets are very different as transparent space in the center of the housing, a single sensor can would be reflected in the process noise matrices.

We assume the actually sense light through more than one lens. Conversely, we cameras are then used to observe ceiling-mounted light-emitting assume the device parameters are constant, and so the elements of diodes LEDs to track the position and orientation of the HiBall. In contrast, the correspond to the velocity estimates in the state 3. If the tracker HiBall sensing unit is the size of a golf ball and weighs only five estimate rate is high enough, poorly estimated device parameters ounces, including the electronics.

The combination of reduced will result in what appears to be almost instantaneous target weight, smaller packaging, and the new SCAAT algorithm results motion. For each of the methods we varied the measurement noise, the measurement frequency, and the beacon position error.

For the multiple constraint methods we also varied the number of constraints beacon observations per estimate N. In each case the respective estimates were compared with the truth data set for performance evaluation. For each beacon exposed and the lenses removed. The sensors, which can be seen filter we used an identical noise covariance matrix through the lens openings, are mounted on PC boards that fold- up into the HiBall upon assembly. See [47] for the complete details.

At each estimate step, and can perform controlled experiments. The cameras provide a single 2D measurement distance estimate obtained from the user and beacon positions in vector, i. The position and orientation elements of the main tracker state For the simulations we generated individual measurement were initialized with the true user position and orientation, and the events a single beacon activation followed by a single camera velocities were initialized to zero.

The beacon filter state reading at a rate of Hz, and corrupted the measurements vectors were initialized with potentially erroneous beacon using the noise models detailed in [8]. Public users can however freely search the site and view the abstracts and keywords for each book and chapter. Please, subscribe or login to access full text content. To troubleshoot, please check our FAQs , and if you can't find the answer there, please contact us.

All Rights Reserved. OSO version 0. University Press Scholarship Online. Sign in. Not registered? Sign up. Publications Pages Publications Pages. Recently viewed 0 Save Search.