Use of Gravity Sensors for Free Space Orientation
VICTOR GARCIA
K T H R O Y A L I N S T I T U T E O F T E C H N O L O G Y
Use of gravity sensors for free space orientation.
Victor Garcia
June 2014
Abstract
This thesis deals with different approaches to filtering the signal output of a three axis accelerom- eter for free space orientation, that is, finding the orientation of a sensor relative to gravity. The plattform this orientation system is to be developed for is a low-power, high-efficiency fixed-point math microprocessor, therefore efficiency and mathematical operation precision are factors that also need to be taken into consideration in this work.
This is an exploratory work which goals are to analize potential filtering solutions for free space orientation with a three-axis accelerometer, develop a tool to validate the theorical analysis, study the repercusions of limited precision math on those algorithms and implement a filtering solution as versatile as possible.
The goal of finding the orientation with the output of an accelerometer and without a priori information is deceptively simple: while in relaxed state and within inertial systems the output of a three axis accelerometer and the direction of gravity is the same, when that hypothesis is taken away measurements output by the accelerometer include both gravity and acceleration due to external forces applied to the system.
The contributions of this thesis are a discussion of potential solutions for free space orientation with unrestrictive preconditions by use of an accelerometer, and an implementation of such solu- tion for the MSP430, popular platform of choice for digital signal processing. The main purpose of such a solution is to improve the precision with which gravity is estimated from the output of the accelerometer. Potential applications of this work are relative position tracking, mapping, positioning systems (for example, within buildings or vehicles, where any other existing positioning system cannot work, such as tunnels for GPS).
Instead of just low-pass filtering the output of the accelerometer, estimations to the state of external forces applied to the system and tracking of changes to those forces are presented. To model that system, the Kalman filter and the Particle filter are introduced and analyzed as po- tential solutions.
This document includes a discussion of both Kalman and Particle filters, implementation of a tool to compare and validate the models to estimate gravity, a discussion of the effect of fixed point math to those models and an implementation of a gravity estimation algorithm that is type and plattform agnosic based on the output of a 3-axis accelerometer.
All relevant code has been included as an appendix to this work.
Keywords: Kalman, Particle filter, Fixed-point math, MSP430.
Inledning
Denna avhandling beskriver olika metoder f¨or filtrering av utsignalen fr˚an en tre-axlig acceler- ationsm¨atare f¨or orientering i ledigt utrymme, det vill s¨aga att finna en sensors orientering i f¨orh˚allande till tyngdkraften. Den plattform detta orienteringssystem ska utvecklas f¨or ¨ar en h¨ogef- fektiv mikro-processor med fixed-point matematik, d¨arf¨or ¨ar ¨aven matematisk operationsprecision och effektivitet faktorer som m˚aste beaktas i detta arbete.
Detta ¨ar ett f¨orberedande arbete d¨ar m˚alet ¨ar att analysera m¨ojliga filtreringsl¨osningar f¨or fri utrymmesorientering med en tre-axlig accelerationsm¨atare, utveckla ett verktyg f¨or att validera teoretisk analys, studera konsekvenserna av att anv¨anda begr¨ansad precisionsmatematik p˚ade al- goritmerna och implementera en filtreringsl¨osning f¨or mikroprocessorerna.
M˚alet med att finna orienteringen med hj¨alp av utsignalen fr˚an en accelerationsm¨atare och utan a priori-information ¨ar bedr¨agligt enkel: I avslappnat tillst˚and och inom interna system ¨ar utg˚angen hos en tre-axlig accelerationsm¨atare och tyngdkraftens riktning densamma, n¨ar den hy- potesen ¨ar borttagen visar m¨atningar att utg˚ang av accelerometern inkluderar b˚ade gravitation och acceleration p˚agrund av yttre p˚afrestningarna p˚asystemet.
Bidragen fr˚an denna uppsats ¨ar en diskussion om m¨ojliga l¨osningar f¨or orientering i fritt utrymme med orestriktiva f¨oruts¨attningar genom anv¨andning av en accelerationsm¨atare, och ett genomf¨orande av en s˚adan l¨osning f¨or fixed-point matematik mikrokontrollerna. Det huvudsakliga syftet med en s˚adan l¨asning ¨ar att f¨orb¨attra den precision med vilken gravitation uppskattas fr˚an utsignalen fr˚an en accelerationsm¨atare. Potentiella till¨ampningar av detta arbete ¨ar relativ posi- tionssp˚arning, kartl¨aggning, positioneringssystem (till exempel inom byggnader eller fordon, d¨ar andra befintliga positioneringssystem inte kan arbeta, till exempel tunnlar f¨or GPS).
Ist¨allet f¨or att bara l˚agpass-filtrera utsignalen fr˚an en accelerationsm¨atare appliceras uppskat- tningar till stadiet av yttre krafter p˚asystemet och sp˚arning av ¨andringar i dessa krafter presenteras.
F¨or att modellera systemet introduceras Kalman-filter och partikelfilter f¨or att analyseras som po- tentiella l¨osningar.
Det h¨ar dokumentet inneh˚aller en diskussion om b˚ade Kalman och Partikelfilter, implemen- tation av ett verktyg f¨or att j¨amf¨ora och validera modeller i syfte att uppskatta gravitationen, en diskussion om effekten av fixed-point matematik f¨or dessa modeller och ett genomf¨orande av en gravitationsuppskattningsalgoritm f¨or mikrokontrollerna utifr˚an utg˚angen hos en treaxlig ac- celerometer.
Relevant kod finns som bilaga till detta arbete.
Nyckelord: Kalman, Partikelfilter, fast punkt matematik, MSP430.
Resumen
Este documento presenta y discute diferentes soluciones para filtrar la se˜nal de salida de un aceler´ometro triaxial con el objetivo de conocer su orientaci´on en el espacio libre, es decir, es- timar la orientaci´on del senson en relaci´on con la gravedad. La plataforma sobre la cual estos algoritmos han de ser desplegados es un microcontrolador de bajo consumo y alta eficiencia, TI MSP430, por ello, la eficiencia y la precisi´on de las operaciones matem´aticas en los diferentes al- goritmos son tambi´en tratadas en este documento.
Este es un trabajo de exploraci´´ on cuyos objectivos son el an´alisis de soluciones para el filtrado de la se˜nal de un aceler´ometro triaxial, el desarrollo de una herramienta para la validaci´on del an´alisis te´orico, el estudio de la repercusi´on sobre los algoritmos de filtrado de la limitada precisi´on en las operaciones matem´aticas, y la implementaci´on de una soluci´on de filtrado para el microcon- trolador MSP430 de Texas Instruments.
Encontrar la orientaci´on de un sensor con la se˜nal de salida de un aceler´ometro es un abjetivo relativamente complejo: mientras el sensor est´a estacionario, la se˜nal de salida es la orientaci´on, pero cuando fuerzas externas son aplicadas sobre el sensor, estas fuerzas contribuyen como ruido al problema que se presenta.
Las contribuci´ones de este documento son una discusi´on sobre potenciales soluciones para el filtrado de la se˜nal de un aceler´ometro triaxial para la orientaci´on en el espacio libre, y una imple- mentaci´on de una soluci´on para el microcontrolador MSP430, plataforma popular para el proceso digital de se˜nales. La principal misi´on de esta implementaci´on es la mejora de la precisi´on con la que se estima la direcci´on de la gravedad mediante el uso de un aceler´ometro triaxial. Aplicaciones potenciales est´an relacionadas con seguimiento de posici´on, mapeado, sistemas de posicionamiento (por ejemplo, dentro de vehiculos o edificios donde otros sistemas no funcionar´ıan, como por ejem- plo GPS en un t´unel).
En lugar de un simple filtro paso bajo para eliminar el ruido introducido por fuerzas externas, estimaciones del estado de las fuerzas externas y filtros para el seguimiento de las mismas son presentados en este documento. Para modelar dicho sistema, se introcucen el filtro de Kalman y el filtro de Part´ıculas, y ambos se analizan como soluciones para este problema.
Este documento incluye una discusi´on de ambos fltros, la implementacion de una herramienta de validaci´on para los mismos, an´alisis del efecto de la introducci´on de ´algebra de punto fijo sobre los modelos, y una implementacion del sistema para el MSP430 de Texas Instruments.
El c´odigo relevante de este trabajo se incluye como ap´endice a este trabajo.
Palabras clave: Kalman, filtro de part´ıculas, ´algebra de punto fijo, MSP430.
Contents
1 INTRODUCTION 1
1.1 Background and motivation. . . 1
1.2 Potential applications, existing technology. . . 2
1.3 Related work. . . 2
1.4 Goals . . . 3
1.5 Thesis structure . . . 4
2 MATHEMATICAL BACKGROUND 5 2.1 Hidden Markov Model . . . 5
2.2 Kalman Filter . . . 6
2.2.1 Introduction . . . 6
2.2.2 Informal description . . . 6
2.2.3 Formal definition . . . 8
2.2.4 Useful definitions . . . 9
2.2.5 Derivation of the Kalman filter . . . 10
2.2.6 Kalman filter summary . . . 12
2.3 Particle Filter . . . 14
2.3.1 Introduction . . . 14
2.3.2 Model and Approximation . . . 14
2.3.3 Sampling Importance Resampling (SIR) . . . 15
3 DESIGN 17 3.1 Device . . . 17
3.1.1 Hardware . . . 18
3.1.2 AT command-set . . . 21
3.2 Software and Simulations . . . 21
3.2.1 Main window . . . 22
3.3 Multidimensional Kalman filter model. . . 23
3.3.1 State vector, coordinate system. . . 23
3.3.2 Propagation matrix . . . 23
3.3.3 Measurement matrix . . . 24
3.3.4 Noise: Variable change, Covariance matrix . . . 25
3.3.5 Adaptive residual check threshold . . . 26
3.3.6 Maneuver noise . . . 26
3.3.7 The filter parameters . . . 27
3.4 Single dimension Kalman filter model . . . 27
3.5 Fixed-point math simulation and type-agnostic implementation . . . 28
3.6 Firmware design . . . 29
4 IMPLEMENTATION AND ANALYSIS 31
4.1 Antialias filter . . . 31
4.2 Particle filter simulation . . . 32
4.2.1 Discussion . . . 33
4.3 Multidimensional Kalman filter implementation . . . 34
4.3.1 Math . . . 34
4.3.2 Data structures . . . 35
4.3.3 Filter parameters . . . 36
4.4 Single dimension Kalman filter . . . 40
4.5 Type agnostic filtering . . . 44
4.6 Discussion . . . 47
4.7 Firmware implementation . . . 47
4.7.1 Power management and communication . . . 48
4.7.2 Timer setup . . . 49
4.7.3 Sampling . . . 49
4.7.4 Development Environment, Discussion . . . 50
5 CONCLUSIONS 51 5.1 Conclusion . . . 51
5.2 Future work . . . 51
A Code 55 A.1 AT Command module. . . 55
A.1.1 Vector.h . . . 55
A.1.2 BoardComProtocol.h . . . 55
A.1.3 BoardComProtocol.c . . . 56
A.2 Type Agnostic Math functions. . . 57
A.2.1 IntegerMath.h . . . 57
A.2.2 IntegerMath.c . . . 59
A.2.3 IntegerMath ScaledInt.c . . . 62
A.3 Type Agnostic Kalman Filter Implementation. . . 64
A.3.1 KalmanFilter 1D int type.h . . . 64
A.3.2 KalmanFilter 1D int type.c . . . 65
A.4 Firmware Implementation. . . 70
A.4.1 main.h . . . 70
A.4.2 main.c . . . 71
A.5 Particle filter MATLAB simulation . . . 87
Nomenclature
+ Magnitude value in next iteration
∼ Magnitude approximation k Discrete instatnt in time Pk Covariance matrix at time k F Propagation matrix
H Measurement matrix Gk Kalman Gain at time k xk State vector at time k yk State prediction at time k
wk Process or maneuver noise at time k Qk Measurement noise at time k vk Measurement noise at time k
Rk Measurement noise covariance at time k E Mean of a magnitude
σx2 Variance of a magnitude φ Azimuth
θ Zenith
φ Azimuth angular speed˙ θ Zenith angular speed˙
∆t Time increment
Tres Residual check threshold
1. INTRODUCTION
1.1 Background and motivation.
Orientation awareness is something we are very used to, in fact, when it comes to an individual, lack of awareness of relative orientation relative to the ground is regarded as exceptional. The reason is environment and behavior related: besides the sensor in human’s inner ear, which among other function acts as a set of accelerometers ([2]), other sensors give valuable input to finding relative orientation in relation to the ground, for instance the eye - we have knowledge of the landscape and its characteristics; and reason - our brain, taught of the world throughout the years - tells us of our own orientation. Yet it is not a perfect system. Two examples come easily to mind: When different sensors disagree, discrepancies may cause problems - some people get dizzy when they get on a vehicle that moves fast and violently; when part of the input is missing we get unsure or wrong of the actual orientation - we wake up in a train wondering which direction it is going, or we lose track of our orientation in a roller-coaster when we close our eyes.
Machines do not have access to the inmense set of tools evolution has given the human brain, nor have they been educated for years on how the world works the way a human has. But you can provide a machine with a subset of the information the human brain has at its disposal: a set of accelerometers can to some extent work as our inner-ear sensor when it comes to detecting forces, and processing of images obtained from a camera could give certain information to extrapolate orientation, such as orientation of observed objects. On the other hand, the same way humans can get mismatched information, so can different sensors.
From all the input a microcontroller can be exposed to, the use of an accelerometer is as a first approach the most appealing: it directly measures the magnitude we are interested in under inertial conditions. Many other sensors that provide orientation or orientation-related information can be added, such as a gyroscope or a magnetic compass - it is the purpose of this work to assess the maximum information that is possible to extract from the output of the accelerometer alone, in a way that can scale, be extrapolated, complemented or enriched by the input of other sensors.
Accelerometers measure the linear acceleration of the system in the inertial reference frame, but in directions that can only be measured relative to the moving system. This is because accelerome- ters are fixed to the system and rotate with the system, but are not aware of their own orientation.
This effect is basically the one felt by any person when he/she experiences any acceleration.
The data obtained with the accelerometer provides information on how the velocity of the sen- sor varies relative to itself, whether it is increasing or deacresing forward, backward, left, right, up, or down, but just relative to the local axis system (local directions) and not the direction relative to the Earth, since it doesn’t know what direction it was facing relative to the Earth to begin with.
This effect is basically the same felt by any person on a train with his/her eyes closed.
By tracking both the current angular velocity of the system and the current linear acceleration of the system measured relative to the moving system, it is possible to determine the linear ac- celeration of the system in the inertial reference frame. One would think this solves the problem
at hand but, calculating angular velocity out of acceleration requires an integration, calculating position requires two integrations, and those operations, besides being time-consuming, suffer from integration drift - more the latter that the first, which would certainly cause error in the measure- ment, furthermore, that error would get bigger with time. This solution is therefore disregarded, and the option taken is to find out a clever way to filter out acceleration components not related to gravity, without any integration, if possible.
Another limitation of the use of embedded system and digital signal processing for the purpose of finding the orientation is precision: orientation is a continuous magnitude, that is to be sampled by the microcontroller, and those samples represent with a finite accuracy the current value of the output. Therefore, sample rate and data type dynamic margin and granularity are also subject to study and analysis for this application.
This document will try to assess wether keeping state for the system aside from that of the measured acceleration adds value and performs better than filters that take into account only the signal from the accelerometer, and wether it is possible to implement such an algorithm in a type- agnostic manner on the MSP430.
1.2 Potential applications, existing technology.
Applications for this work are very abundant and cross-disciplinary. Orientation is crucial in many applications such as robotics, auto-motion, orientation devices of all sorts, and many gadgets that have been or are to be designed, and require the knowledge of the relative position of the gadget itself. Some projects have shown in their conclusion that their goal has been satisfied provided the direction of g is determined or stated that their performance would be increased if that data was available, such as Andreas Eriksson’s Master Thesis at KTH ([5]) dealing the design of a device to measure the weigh of loaded trucks and adjust motor and breaking efficiency accordingly, even when the truck is moving along slopes. This would implicitly add value to that work.
More information on applications of orientation awareness can be found, such as the following:
in [16] astronauts’ orientation awareness is shown to improve by adding vibration in their torso to indicate then the direction of down; Karonlinska Institutet (Stockholm) works on applying sensor information to monitor patients orientation in real time; and orientation awareness is also a key component of a recently popular technology, augmented reality, as shown in [14].
There are many devices in the market commonly used that have the capacity of detecting the direction of Earth gravitational field, such us almost every smartphone sold today, but no open source adaptative filtering solution ad-hoc for the MSP430 microcontroller could be found at the date this work started.
1.3 Related work.
This problem can be presented as tracking of a moving target. The direction of the gravity vector can be fully defined as a vector from the center to a point of a sphere of normalized radius, and its evolution with time can be modelled as the evolution in time of a point on a shphere of normalized radius. Extensive work has been done on filtering for tracking purposes by applying a Kalman filter such as [13], a particle filter [6]. [4] indicates Kalman filters and Particle filters are the best statistical models for tracking purposes, and Kalman filters are the core of the vast majority of radar tracking systems, their task being associating measurements of a target in different frames
into a single object maneuver.
Applications of the Kalman filter are numerous, and literature on this filter is extensive, start- ing with its original creator’s work [11].
Figure 1.1: Particle filter tracking movement in a video
Monte-Carlo algorithms such as Particle filters are applied to problems where inputs have a large number of degrees of freedom, when inputs are hard to describe statistically, when little is known about the problem at hand. By randomly sampling the possible inputs for the problem, the output distribution is roughly approximated. In some cases, this algorithm beats Kalman filter, some other cases both are equivalent, sometimes Particle filter gives out worse results. Among its applications, Particle filters have widely been used in tracking a moving object in a video (1.1), problem which does not appear to resemble this one, but as will be shown, actually does.
About hardware, there are many commercial accelerometers, microcontrollers, sensor interfaces, such the Arduino device family [1]. The one chosen for simplicity and availability reasons was the Wasa Board ([17], Figure 1.2), which features accelerometers and an interface to communicate with them, plus a reprogrammable chip, allowing implementation of the algorithms.
1.4 Goals
The goals for this specific master thesis are:
1. Evaluation of a Kalman filter and a particle filter as solutions for the estimation of the direction of gravity from the input of a 3-axis accelerometer.
Figure 1.2: Wasa board connected through it’s USB interfaces
2. Discussion the proposed algorithms, its advantages and disadvantages.
3. Implementation of a validation and simulation tool for the proposed algorithms.
4. Study of the performance of the algorithms under different conditions.
5. Development of a type-agnostic implementation of a filtering mechanism for the output of a 3-axis accelerometer.
1.5 Thesis structure
The remainder of this report is organized as follows:
Chapter 2 will introduce the mathematical tools used in the whole thesis, and where appropri- ate, will deduce and explain them in detail. These are the particle filter and the Kalman filter.
Chapter 3 will show the design of different models to describe the orientation estimation prob- lem; the design of the validation and simulation tool, and the communication between this tool and the hardware; the design of a Kalman filter, and the design of the simulation of data type precision and granularity that integer or fixed point math processor will introduce.
Chapter 4 will present the process and details of the implementation of the designed model in its different stages, and and discussion of the results obtained in each state of the implementation.
Chapter 5 will summarize the conclusions and openings derived from this work.
Appendix A contains all relevant code used in this work.
2. MATHEMATICAL BACKGROUND
Being aware of the Orientation of an object in relation to Earth is equivalent to knowing the direction of Earth’s gravitational field in relation to the object’s local axis; in other words, being aware of the orientation means knowing the variable change required to go from inertial to local coordinate system, or it’s inverse. Once one is known, so it the other. Thus, given an inertial co- ordinate system, the orientation of the object is the output of this problem. Now, this orientation may be represented by a unitary vector in the three dimensional euclidean space, which is defined by origin (0,0,0) and a point in the unitary sphere centered at the origin.
A point in euclidean space belonging to a sphere of a given radius can also be represented with two polar coordinates, azimuth - between −π and π and zenith - between 0 and π: f (x, y, z) = f0(R = cte, φ, θ). With this variable change we reduce the dimension of the problem, it becoming tracking of a target in a rectangular domain (φ, θ), examples of which we can find in many fields, which get solved by several algorithms such as those that follow.
The sensor whose output we need to filter is a 3-axis accelerometer, which measures acceleration produced by gravity plus accelerations produced by other forces. For this problem, accelerations produced by other sources are regarded as noise to our process. The goal is to filter out the noise.
Several solutions for the filtering problem are studied in this thesis, and their performance dis- cussed. They are introduced in this chapter, and are Particle Filters, and Kalman Filters, which will also be compared with a simple Finite Impulse Response filter.
Kalman filter and Particle filter are considered as potential solutions for this problems since both try to adaptatively minimize the error of expected measurements in time and evolve to adapt with the state of the system. They take into account the relation between a measurement and the next, and provide with mechanisms to model random noise of the nature of that which will affect the measurements.
If the estimation of g at a given time t0 is considered independent of the measurements taken at any t < t0 we are disregarding a priori information, and the problem turns impossible to approach. This work assumes this is not true: the orientation of the device as it evolves depends on the previous situations, orientation cannot change at arbitrarily large speed.
Additionaly, Markov Models and Hidden Markov Models will be briefly presented as an ap- proach to the Kalman filter to provide an understanding of why it is considered a suited choice for this application.
2.1 Hidden Markov Model
A Markov Model is an stochastic model used that applies to system with a finite amount of states, where the probability distribution of the next state the system will transition to at any given time
depends only of the current and previous states.
A Hidden Markov Model, from now on HMM, is an statistical model in which the system being modeled is considered to be a Markov Model (state machine + transition probabilities) where the state is not fully observable, but magnitudes related to the state of the system are. The input to this system is the series of observations, and its output is the hidden parameters obtained from the observed state transitions. This has applications to for instance, speech recognition, where the samples of the sound pronounced are observable, but the phonem that the speaker was pronouncing is not. Such a system can be used to determine which series of phonems were most likely to have produced the observation measured.
In other words, HMM make intelligent guesses about long sequences of patterns. If the elements of this sequences were independent from each other, each decision could be made independently, but they are correlated, thus the optimal decision cannot be made independently. Since the mea- surements are affected with noise, the sequence is hidden, being only accessible the influenced variables, from which we estimate the series of states.
In our model, the hidden states would be the direction of g, and the measurements are the output from the sensor. If the domain of the direction of g was discretized to a set of directions (states), with a series of noisy obsrvations we could estimate which is the most likely direction for g.
HMM gives a simple mathematical structure to the problem, but describes the problem at hand with critical limitations, which proves enough to disregard it as a solution: since the number of states is finite, the 3D space needs to be discretized, and though a greater number of states gives better sampling and more precision, it increases exponentially the computing effort to be made;
furthermore, there is a limit on how long sequences can be, so any algorithm would apply in sets of samples acquired in a certain time, to be then somehow reset.
A Kalman Filter can be presented as an analogue of the HMM : observations of magnitudes related to the state of a system are made, and the state of the system is estimated from them, but the domain of the possible states is continuous.
2.2 Kalman Filter
2.2.1 Introduction
The Kalman Filter ([11]) provides a recursive solution to the linear optimal filtering problem, and applies to both stationary and non-stationary problems. The solution is recursive in the sense that it just needs the previous estimate and the new input, to calculate the next estimate state, and therefore just the last state needs storage. Furthermore, it is more efficient when it comes to computing than studying the whole set of previous states.
2.2.2 Informal description
A Kalman filter is, in plain words, a system that keeps track of a variable or more than one variable linked linearly to one another, by receiving and processing samples of at least one of them. For example it may be tracking position, speed and acceleration of a target, getting data of just the position, or both the position and velocity, or just any combination of the above. It does so re- cursively in three stages: Keeps track of its last output, estimates a new output given the last one and variables being kept track of, and finally corrects the statistical variables and the estimation
according the difference between the estimation and the measurement with the Kalman Gain.
Let there be a Kalman filter tracking the position and velocity of a target, just by measuring its position. It would set the initial velocity to a reasonable value according to the experiment. The filter linearly estimates the new position, which then would be measured, and by considering both (estimate and measurement), it would update the new position and velocity plus the statistical variables for the next estimation with a correcting factor called Kalman Gain.
A Kalman filter is said to be adaptive if its behavior changes with time, in the example, if the behavior of the filter changes when a maneuver of the target is detected.
Let ∼ denote estimation, k a discrete instant in time, and + denote a magnitude corresponding to a next state, then recursive Kalman filter loop for the example described previously is described in Figure 2.1: an state vector xk with, for this particular example, two elements - position and velocity; stochastic variables for both stored in a 2x2 matrix specific for this example Pk, squared 2x2 transition matrix to go forward in time (estimate in time, equivalent to the two linear ecuations for position and speed respectively) F, and the measurement matrix to get just the position H.
Within the loop the gain to update estimations would be calculated when measurement arrives, Gk.
Figure 2.1: The Kalman Filter.
Derivation of the algorithm consists in figuring out an analytical expression for Gk, since the rest can be derived from the problem and initialized accordingly. Loop goes by estimating - calcu- lating gain - updating state & covariance - loop.
2.2.3 Formal definition
Figure 2.2: The dynamic system model.
The system is modeled according Figure 2.2, as linear, discrete-time, dynamical system. The state vector, or sate, denoted by xk, is defined as the minimal set of data that uniquely describes the behavior of the system; the subscript k denotes discrete time. Typically, with the exact same idea as in the last section, the state is unknown, and is predicted from a set of data or observation, denoted by yk. When we put the diagram in the figure in writing, we get the following equations:
1. Process equation
xk+1= Fk+1,kxk+ wk (2.1)
where Fk+1,k is the transition matrix taking the state xk from time k to time k + 1. The noise wk is assumed to be additive, white gaussian, with zero mean and covariance matrix Qk equal to
EwnwTk =
Qk : n = k,
0 : n 6= k, (2.2)
T denoting transposition and E denoting mean. The dimension of this space will be from now on denoted as M .
2. Measurement equation
yk = Hkxk+ vk (2.3)
where yk is the observable at time k and Hk is the measurement matrix. The additive noise vk is considered to be additive, white and gaussian, with zero mean and convariance Rk
EvnvkT =
Rk : n = k,
0 : n 6= k, (2.4)
Furthermore, the measurement noise vk is uncorrelated to the process noise wk. The dimen- sion of the measurement space will now be denoted with N .
The Kalman filtering problem is rephrased with that notation as follows: Use the entire ob- served data, consisting of vectors y1. . . yk to find for each k ≥ 1 the minimum mean-square error estimate of the state xi.
The problem is called filtering if i = k.
The physical meaning of the variables defined in the scheme and the equation follows:
1. xk: state vector at time T = k, a set of variables being kept track of, in order to give an output.
2. Fk+1,k: transition/prediction matrix, operator used to update the variables being kept track of.
3. Hk: measurement matrix, operator that extracts the variable we want from the set of vari- ables being kept track of.
4. vk: measurement noise, added white gaussian due to imperfection of every sensor, to be consulted on datasheet.
5. wk: process noise, added white gaussian due to the nature of the experiment, parameter of the filter.
2.2.4 Useful definitions
Before deriving the Kalman filter, a series of definitions that will be used later will now be presented.
Supose we are given the observable
yk = xk+ vk
where xk is the unknown signal and vk is the additive noise component. Let ˆxk denote the a posteriori estimate of the signal xk, given the observations y1, . . . , yk. To find the optimal estimate we need to discriminate the different estimates, and therefore a measurement needs to be defined, a cost funtion which shall not be negative and a non-decresing function of the estimation error
˜
xk = xk− ˆxk
That is the mean-square error defined by Jk = Eh
(xk− ˆxk)2i
= E ˜x2k
We will also invoke these theorems without proof, since they can be found in the bibliography [15],
Theorem 2.2.1 (Conditional mean estimator) If the stochastic processes {xk} and {yk} are jointly Gaussian, then the optimum estimate ˆxk that minimizes the mean-square error Jk is the conditional mean estimator:
ˆ
xk= E [xk|y1, . . . , yk]
Theorem 2.2.2 (Principle of orthogonality) Let the stochastic processes {xk} and {yk} be of zero means; that is,
E [xk] = E [yk] = 0 ∀k Then
(i) the stochastic process {xk} and {yk} are jointly Gaussian; or
(ii) if the optimal estimate ˆxk is restricted to be a linear function of the observables and the cost function is the mean-square error, then the optimum estimate ˆxk givn the observables y1, . . . , yk is the orthogonal projection of xk on the space spanned by the observables.
2.2.5 Derivation of the Kalman filter
To derive the Kalman filter, we use the reasoning by the author R.E. Kalman, that can be found in the bibliography ([11], [10]).
Lets suppose a measurement of a linear dynamic system as the one described in Figure 2.2, according to the equations 2.1 and 2.3, has been taken at time k. What we need to do is use the information in yk to update the estimate state xk. Let ˆx−k be the a priori estimate of the state, available already at time k. We may now express the a posteriori estimate as a linear combination of the a priori estimate and the measurement,
ˆ
xk = G(1)k ˆx−k + Gkyk (2.5)
where the multiplying fators G(1)k and Gk are to be determined. Defining the state-error vector as
˜
xk= xk− ˆxk (2.6)
and aplying then the theorem 2.2.2, we may write
E˜xkyTi = 0 ∀i ∈ [1, 2, . . . , k − 1] (2.7) Using Eqs.2.3, 2.5 and 2.6 in 2.7, we may now write
Eh
xk− G(1)k xˆ−k − GkHkxk− Gkwk
yTi i
= 0 ∀i ∈ [1, 2, . . . , k − 1] (2.8) Now, the process noise wk and measurement noise vk are uncorrelated, thus EwkyTi = 0, rearranging terms we get
Eh
I − GkHk− G(1)k
xkyTi + G(1)k xk− ˆx−k yTii
= 0 (2.9)
where I is the identity matrix. We can also note form orthogonality that E
xk− ˆx−k yTi = 0 (2.10)
thus Eq.2.9 gets rewritten as
I − GkHk− G(1)k
ExkyTi = 0 ∀ i = 1, · · · , k − 1. (2.11) which can only be achieved if
I − GkHk− G(1)k = 0 (2.12)
since ExkyTi 6= 0 for arbitrary values of the state xk and observavble yi. Equivalently,
G(1)k = I − GkHk (2.13)
which substituted into Eq.2.5 gives us the a posteriori estimate of the state at time k, ˆ
xk= ˆx−k + Gk yk− Hkˆx−k
(2.14)
which, due to analogy to any linear system, leads us to name Gk as Kalman Gain. There on, it remains to calculate an explicit formula for this gain.
Since
E(xk− ˆxk) yiT = 0 (2.15)
and due to the principle of orthogonality, it follows that
E(xk− ˆxk) ˆyiT = 0 (2.16) where ˆx denotes again estimate given previous measurements. We now Define the innovations process as
y˜k= yk− ˆyk (2.17)
This represents a measure of the new information contained in the last measurement taken, and may be rewritten as
˜
yk = yk− Hkˆx−k
= Hkxk+ vk− Hkxˆ−k
= Hk˜x−k + vk
Thus, substracting Eq.2.16 from Eq.2.15 and using the definition of innovations in Eq.2.17, we write
E(xk− ˆxk) ˜ykT = 0 (2.18) Now, using Eq.2.3 and Eq.2.14, we may express error vector as
xk− ˆxk = ˜x−k − Gk Hk˜x−k + vk
(2.19) or
xk− ˆxk = (I − GkHk) ˜x−k − Gkvk (2.20) and then, substituting the innovations and the state-error vector into Eq.2.18 to get
E(I − GkHk) ˜x−k − Gkvk Hk˜x−k + vk = 0 (2.21) Since the measurement noise vk is independant of the state mathbf xk and therefore it is also independant of the error ˜x−k, the expectation gets simplified as
(I − GkHk) Eh
˜
xk− ˜x−k
xk− ˆx−kTi
= 0 (2.22)
We now define the a priori covariance matrix as P−k = Eh
xk− ˆx−k
xk− ˆx−kTi
= E˜x−k · ˜xT −k
(2.23) and with the definitions in Eqs.2.4 and 2.23 we rewrite Eq. 2.22 as
(I − GkHk) P−kHTk − GkRk= 0 (2.24) and we solve that equation for Gk to get the desired result,
Gk= P−kHTk HkP−kHTk + Rk−1
(2.25)
denoting [·]−1 the inverse of the matrix between brackets.
To complete the recursive estimation procedure we shall consider the error covariance propa- gation, i.e. the effect of time on the covariance matrix of estimation errors, which will get done in two stages of computation:
1. The a priori covariance matrix P−k at time k is defined by Eq.2.23. Given P−k, compute the a posteriori covariance matrix Pk, defined at time k as
Pk= E˜xk˜xTk =h
(xk− ˆxk) (xk− ˆxk)Ti
(2.26) 2. Given the old a posteriori covariance matrix Pk−1, compute the updated P−k
The first stage goes as follows:
Substituting Eq.2.20 into Eq.2.26 and also noting that the noise process vk is independant of the a priori estimation error ˜x−k, we obtain
Pk = (I − GkHk) P−k (I − GkHk)T+ GkRkGTk (2.27) Expanding Eq.2.27 and using Eq.2.25, we can simplify the update formula for the a posteriori covariance matrix as
Pk = (I − GkHk) P−k − (I − GkHk) P−kHTkGTk + GkRkGTk
= (I − GkHk) P−k − GkRkGTk + GkRkGTk
= (I − GkHk) P−k
For the second stage, given that ˆx−k = Fk,k−1ˆxk−1, and given Eq.2.1 we write the a priori estimation error as
˜
x−k = xk− ˆxk
= (Fk,k−1xk−1+ wk−1) − (Fk,k−1ˆxk−1)
= Fk,k−1(xk−1− ˆxk−1) + wk−1
= Fk,k−1x˜k−1+ wk−1
Using this into Eq.2.23 and since wk is independant of ˜xk−1, we get the desired relationship, P−k = Fk,k−1Pk−1FTk,k−1+ Qk−1 (2.28) defining the dependance of the a priori covariance matrix with the old a posteriori covariance matrix.
2.2.6 Kalman filter summary
The computation steps of the Kalman Filter derived in the previous section will be summarized here:
1. State-space model:
• xk−1= Fk+1,kxk+ wk
• yk= Hkxk+ vk 2. Initialization: For k = 0,
• ˆx0= E [x0]
• P0= Eh
(x0− E [x0]) (x0− E [x0])Ti 3. Computation:
• State stimate propagation: ˆx−k = Fk,k−1ˆxk−1
• Error covariance propagation: P−k = Fk,k−1Pk−1FTk,k−1+ Qk−1
• Kalman gain matrix: Gk= P−kHTk HkP−kHTk + Rk−1
• State stimate update: ˆxk = ˆx−k + Gk yk− Hkˆx−k
• Error covariance update: Pk = (I − GkHk) P−k
2.3 Particle Filter
2.3.1 Introduction
Monte Carlo methods ([8]) are a class of computational algorithms relying on repeated random sampling to compute results. They are specially useful in studying systems with a large degree of freedom (such as the one presented), and modeling phenomena with significant uncertainty in its inputs.
The term Monte Carlo method describes a range of methods that generally follow this pattern:
1. Define a domain of possible inputs
2. Generate inputs randomly from the domain using an specified probability distribution 3. Perform deterministic computation using the inputs
4. Aggregate the result of the individual computations into the final result.
Particle filters are also known as sequential Monte Carlo methods or SMC. One of its applica- tions, that suits here, is estimating the state of a system that evolves with time. They are often presented, as in this case, as an alternative to Extended Kalman filter, with the advantage that, with sufficient samples, they approach the Bayesian optimal estimate ([12]), thus making it more accurate in theory.
2.3.2 Model and Approximation
Particle methods assume the following conditions:
1. x0, x1, . . . is a first order Markov process with initial distribution p(x0)
2. The observations y0, y1, . . . are conditionally independant provided that x0, x1, . . . are known, that is, each yk only depends on xk
Particle methods generate a set of samples to approximate the filtering distribution p(xk|y0, . . . , yk).
With P samples, expectations are approximated by the expression Z
f (xk)p(xk|y0, . . . , yk)dxk ≈ 1 P
P
X
L=1
f (x(L)k ) (2.29)
Let x be the observation of a given process y, then the particle filter is fully defined by the following parameters:
1. Initial state x0: The initial state of the system is known.
2. System Noise Covariance (Maneuver noise): σy2: The uncertainty of the evolution of the system.
3. Measurement Noise variance (Measurement noise): σ2x: The uncertainty of the measurement.
4. Number of particles N : The amount of particles used to estimate the a posterior distribution
Given those parameters, the computation of the particle filter follows:
1. Initialization: A set of P particles is randomly drawn on the domain, and their weights set to P1
2. Loop:
(a) Particle update: move each particle to best estimate its next state using the modelled system manuever distribution.
(b) Observation: acquire a new sample.
(c) Assign weight: evaluate how likely each resulting particle is likely to be the actal state.
for L = 1, . . . , P ˆw(L)k = wk−1(L)p(yk|x(L)k )
(d) Normalization: The sum of the probabilities of the state being one of the particles needs to add to 1.
wk(L)= wˆ
(L) k
PP J =1wk(L)
(e) Estimation: calculate the mean of all the particles particle, weighed.
2.3.3 Sampling Importance Resampling (SIR)
The original particle filtering algorithm introduced by Gordon, N. J. in 1993 ([9]) is the most commonly used particle filter implementation. It approximates the filtering distribution as a set of P particles weighed by an importance function. When the importance function is the a priori transition probability p(xk|xk−1), SIR is also known as bootstrap filter or condensation algorithm (conditional density propagation).
The idea is to fill the domain with particles, assign weights to them according to the probability they have to be the right output given a certain input, and predict or simulate their movement in time within the domain for each loop. For each sample input into the system, weights of the particle change, thus the heavier particles should always be around the output of the system. After updating the weights of the particles, one of them is randomly taken as the output, taking into account the estimated output distribution.
Resampling is introduced to avoid degeneration of the algorithm, which happens when all but one particle has probability very close to 0. To resample, a new set of P particles are extracted from the original set with probabilities proportional to their weight.
A description of the Particle filter with SIR follows:
1. Initialization: A set of P particles is randomly drawn on the domain.
2. Actualization: for all the particles, update the weights:
for L = 1, . . . , P ˆw(L)k = wk−1(L)p(yk|x(L)k ) 3. Normalization: for each particle,
w(L)k = wˆ
(L) k
PP J =1wk(L)
4. Resampling: estimated the effective number of particles as Nˆef f = 1
PP L=1
wk(L)2 (2.30)
, if Nef f is less than a given threshold, the particles are resampled according their weight.
Initialization step is performed once, steps 2 to 4 loop for each new measurement taken. In some cases an additional step is added - Prediction. For example, in tracking applications, particles can be moved according to a predicted pattern before resampling, to try helping the process by situating the particles in positions with more weight.
Figure 2.3: The Particle Filter.
3. DESIGN
Once math is presented, a series of models were design, starting with the most generalistic model and then adjusting and simplifying the model to satisfy the different needs that arise as we get closer to the hardware where this filter is to be deployed.
The first approach is a general model for the tracking problem presented: A multidimensional Kalman filter. A variable change is introduced and its implications analyzed. Assuming uncorre- lated dimensions, that general model can be broken down to a number of simpler Kalman filters, one for each dimension filtered. They are the azimuth and the zenith.
The calculations of azimuth and zenith requires trigonometric calculations. There are several libraries that allow the MSP430 to perform the calculations needed, and a look-up table is always a solution, but a Kalman filter for each component of the acceleration before the variable change is introduced would eliminate the need for trigonometric calculations.
The filtering process itself requires multiplication, addition, division and substraction of real values and matrixes. These operations where performed in the model with floating point math, which in the case of the MSP430 needs to be simulated, it is a fixed point math processor. That leads to a very suboptimal implementation. Therefore, a portable type-agnostic undidimensional Kalman filter is designed, where the limitations of fixed point math can be simulated.
All the models developed meed to be tested, compared and validated. To that end, a simula- tion/validation tool was designed. This tool requires a communication interface to the measurement acquiring filter.
This chapter will present the description and design of the tools implented throughout this process:
1. Description of the hardware used and communication interface to obtain data from the hardware.
2. Design of a general multidimensional Kalman filter model suitable for estimation of gravity.
3. Simplified model: Kalman filter for a 1-dimensional magnitude.
4. Type-agnostic model: Kalman filter for a 1-dimensional magnitude that can be used with any data type.
3.1 Device
The piece of hardware chosen for this project is WASA board ([17]), a small PCB board with a microprocessor and various sensors mounted on it, one among them being a three-axis accelerom- eter; as well as a USB interface to communicate the results to the computer. The accelerometer
provides with acceleration data to the microprocessor, which samples it with a 12-bit unipolar Analog-To-Digital Converter (ADC) and then processes that data, to send it to the computer on request following AT protocol. In this chapter, the hardware, communication protocol, and the programming of the microprocessor for data processing will be described.
3.1.1 Hardware
Wasa board
According to [17], The Wasa board is a device that will allow you to easily add sensors to existing computing platforms such as your laptop or PDA. It is a piece of hardware that supports sensors, access to them, and access to a programmable microcontroller. It also provides the necessary interface to reprogram that microcontroller, and since the both the code and the hardware are open-source, we are free to add further functionality to it.
To accomplish the goal of this paper, the important components present in the board are listed here:
1. Microcontroller 2. Accelerometer 3. Anti-aliasing filter 4. Programming interface 5. USB interface
6. Power interface Microcontroller
According to the schematics in [17], the microcontroller mounted on WASA Board is the MSP430f2419 by Texas Instruments, a Mixed signal microcontroller. It is a low-voltage, low-power consumption, 16-bit RISC architecture microcontroller. It features internal DMA, 12-bit A/D and D/A convert- ers, 2 timers, On-Chip comparator and four Universal Serial Communication Interfaces (USCIs).
The communication interfaces are used to send data back and forth from and to another device, in this case a computer, through the USB interface built on the WASA Board. The internal 12-bit ADC is used to sample the acceleration data. Other features are used to program specific functions for the purposes of this paper, described in the next chapter.
Accelerometer
According to the schematics in [17], the accelerometer used on the WASA Board is an MMA7260, by Freescale semiconductors. It is a three-axis, low-g capacitive micro-machined accelerometer, featuring selectable sensitivity (1.5g to 6g).
Its principle of operation is based on measurements of the capacitance of variable capacitors: a g-cell, or capacitive sensing cell can be modeled as a set of two beams plus a moveable beam in the center, attached to a mass. As the mass moves due to acceleration, so does the beam, and thus the distance to the fixed beams will increase on one side the same it decreases on the other. The value of the capacitance between the two fixed beams and the center beam is inverse proportional to the distance, therefore by measuring the capacitance the value of the acceleration is obtained.
This also explains why the accelerometer outputs g when standing still, which is counter-intuitive
Figure 3.1: Model of a capacitive sensing cell.
for most people at first approach to accelerometers.
The g-cell also features signal conditioning, giving at the output a signal with approximate dynamic range slightly lower than the supply voltage.
As stated in the datasheet, the accelerometer introduces noise at 0.1Hz to 1kHz with RMS equals to 350µg/√
Hz.
Anti-aliasing filter
Since the output signal of the accelerometer is not bandwidth-limited, if sampled and reconstructed, aliasing artifacts with appear. To avoid this phenomena, a low-pass filter is built on the WASA Board to filter out high-frequency components of the measurements, making it bandwidth-limited, and thus allowing a sampling at Nyquist frequency or higher not to suffer from aliasing.
Figure 3.2: RC low-pass filter
The footprint of the board allows placement of three RC low-pass filters, one for each com-
ponent (fig. 3.2). This is the simplest low pass filter you can build. It provides a flat pass-band response form 0Hz to the cut frequency, and a a power roll-off of 20dB per decade in frequency after that, which means signal amplitude gets reduced to half when frequency doubles.
The accelerometer datasheet ([7]) recommends a 1KΩ resistor and a 1µF capacitor to filter out noise originated by its own clock source, which gives a cut-off frequency a little higher than 1.5KHz. It also indicates the bandwidth response of the output signal for each axis, 300Hz at 3db decay for X and Y axis, 150Hz for Z axis. Sampling rate for this project is going to be lower, so the signal needs conditioning.
Figure 3.3: RC low-pass filter frequency response The cut frequency is obtained from this expression:
fc = 1 2πRC and it’s frequency response is shown in fig.3.3 Programming interface
A 14-pin programming interface is placed on the WASA Board to alter the programming code of the microcontroller. Dedicated software, loader and wiring is provided by the manufacturer of the microcontroller.
USB and power interface
USB port is used to send back and forth data between the computer and the device, using a serial communication standard. Though data from/to the computer goes through USB port, a dedicated chip from Future Devices plus its dedicated drivers translate the data stream so that it is under- standable by the UART module of the microcontroller.
The board microcontroller can also be reprogrammed through this port, but debugging capa- bilities such as step-by-step running or variable tracking are not supported.
3.1.2 AT command-set
The AT commands have their origin in 1977, and were developed to provide a commercial mo- dem of automatic dialing capabilities, and automatically change parameters of communication.
Prior to them, automatic dialing was done by a semi-dedicated point to point link. The arrival of micro-computing made this solution no longer appropriate, since the controlled may not have had availability of a certain port to exclusively dedicate it to dialing purposes.
Hayes Communications introduced the concept of reusing the existing dataline for both pur- poses, communication and command-line. By appending a special string at the beginning of a command, the device would interpret it as a command and not a data stream, and treat it accord- ingly. This was soon adopted by almost every modem manufacturer. Several flaws where found, such as data streams containing the special string, which was fixed by requiring a specific idle time after the code word to enter command-mode.
Different manufacturers started then adding their own set of commands, and many decided to stop using the specific idle-time to sign command, which ended up leading to denial-of-service attacks when modems started being used widely to access the internet. Eventually, manufacturers came back to a standard closer to the original set of Hayes commands, thus becoming it popular and then universally accepted.
For the same reasons the protocol was adopted in the first place, for this project the data line (chips UART) was multiplexed to transmit both data and commands. The list of standard AT commands was extended to support the functionality of the device being built. The basic code for the AT interpreter was already written into the microcontroller by the time this project started, plus some extended AT commands to access the accelerometer in all its settings.
Additional extended AT commands were implemented tp allow the device to constantly send acceleration data at a precise rate and not just when asked to, and to return both the measured and filtered magnitudes simultaneously:
1. + (Extended):
(a) +O (sensor Object):
i. +OAW: Weak setting Acceleration request.
ii. +OAL: Low setting Acceleration request.
iii. +OAM: Medium setting Acceleration request.
iv. +OAL: Strong setting Acceleration request.
v. +OAO: Turn off the accelerometer.
vi. +OAF: Both current measurement and filtered measurement.
3.2 Software and Simulations
Communication with the Wasa Board can be achieved by the terminal, just by sending data and listening on a specific port. To make simulation and output observation easier throughout the develpment of the different models, a custom communication terminal was developed. This piece of software runs on Mac OSX - the plattform at my disposal - but can be easily ported to other platforms, and features read/write capabilities to the port the Wasa Board has been plugged into.
This piece of software, from now on Accelerometer Output Software, was designed ad-hoc for reading and interpreting simulated or measured orientation data. It supports continuous periodic requests to the Wasa board and interprets and plots both the measurement obtained by the board
and the filtered output.
Additionally, the Accelerometer Output Software is capable of saving measured and filtered data as well as filter parameters used to acquire them, in raw text format readable by MATLAB, for further analysis or plots.
Figure 3.4: Software’s main screen
3.2.1 Main window
The software’s main window is divided into two different parts: On the left, An OpenGL display, a text field with a Send button under it; on the right, a set of radio buttons and switches (see Figure 3.4).
The OpenGL display shows both measured and filtered orientation data in relation to −z Axis, fixed in the center of the screen. It provides user friendly and clear feedback of the effects filtering has on the original signal.
On top of it lines of text are shown: up is the last inclination measurements in degrees on both axis (azimuth and zenith), donw is the last log entry given out by the board.
On the right side of the window, a group of settings can be adjusted. The first set of radio buttons lets the user change the model being simulated by this software:
1. Unfiltered output.
2. Multidimensional Kalman filter.
3. Set of 2 Kalman filters for zenith and azimuth.
4. Set of 3 filters for X, Y, Z components of acceleration.
5. Fixed-point math simulation of a set of 3 filters for X, Y, Z components of acceleration.
6. FIR filtering for X, Y, Z components of acceleration.
7. Fixed-point math simulation of FIR filtering for X, Y, Z components of acceleration.
Finally, buttons are provided to quit the application, see the whole log of this session, start and stop recording the output of the simulation as raw text for further analysis on MATLAB.
3.3 Multidimensional Kalman filter model.
The data required for each stage of the Kalman filter is obtained in this section for this specific problem, and the input parameters are described. In other words, the scheme in Fig. 2.1 is par- ticularized for our own purposes.
3.3.1 State vector, coordinate system.
The Kalman filter for this specific application is designed to track position and velocity of a target in a spherical surface, thus the state vector must be a matrix with a single column and four elements [3],
X =
θ θ˙ φ φ˙
(3.1)
where θ is the inclination or zenith, φ is the azimuth and ˙φ and ˙θ are their first derivative (Fig.
3.5).
The measurement given by the accelerometers are linear, thus a variable change is introduced, φ = atan2y
x
θ = acos
z kvk
3.3.2 Propagation matrix
The propagation matrix is the matrix that needs to update those variables in time assuming the speed or first derivatives stay constant, that is,
θt+∆t= θt+ ˙θ∆t θ˙t+∆t= ˙θt
φt+∆t= φt+ ˙φ∆t φ˙t+∆t= ˙φt
where ∆t is the time between samples, that is ∆t =F 1
sample. Thus the propagation matrix is
F(∆t) =
1 ∆t 0 0
0 1 0 0
0 0 1 ∆t
0 0 0 1
(3.2)
Figure 3.5: Coordinate system for the tracking problem
and
Xt+∆t= F(∆t) · Xt
3.3.3 Measurement matrix
The Measurement vector is the matrix holding the output data from the filter, so it takes the following form
Z =
θ φ
so there is a need of an operator to take out the data we are interested in from the state vector and format it as the masurement vector, that is
Z =
θ φ
= H · X = H ·
θ θ˙ φ φ˙
.
That is the Measurement matrix, for this specific problem, H =
1 0 0 0
0 0 1 0
(3.3)
