[ieee 2009 international conference on artificial intelligence and computational intelligence -...

5
Survey of Kalman Filters and Their Application in Signal Processing Thanh Binh Ngo Hung Lan Le, Thanh Hai Nguyen Faculty of Electrical and Electronics, University of Communication and Transport (UCT), Hanoi, Vietnam E-mail: [email protected] , [email protected] , [email protected] Abstract During the investigation and evaluation of movement, we are interested in the filter and in smoothing the state of measurement at the previous times, called smoother RTS (Rauch-Tung-Striebel), in order to have a smooth orbit of the movement. Moving objects require an essential time period to speed up to achieve the necessary speed. Thus the obtained measurement and predicted values cannot be beyond the given range in spite of the impact noises. Therefore, in this paper, some limit functions/values are derived to reduce significantly the measurement and evaluation values, to reduce the predicted values before give data to the filter and smoother, in order to remove any unusual values. Consequently, the response of micro-processing systems can be improved. 1. Introduction This paper presents some tools to develop the application of Kalman filters in digital signal processing, including Filters (02 models: Extended Kalman Filters EKF1, EKF2) and Smoothers (Rauch-Tung-Striebel and Extended Kalman Smoothers). Based on these surveys, we make the evaluation of application in practice. 2. Estimation and Kalman filter A time-invariant linear model described by continous- time formula as follows: ) ( ) ( ) ( t Lw t Fx dt t dx (1) Where : x Initial conditions: x(0)~N(m(0),P(0)) x F and L : the matrix with features of model. x w(t ) : White Noise with the strength of spectrum is Q c. The classical Kalman Filter that gave a linear model above was introduced by Rudolph E. Kalman (1960). He gave a recursive solution (closed form) to estimate time - linearization discrete dynamic systems. Kalman Filter encompasses 2 steps: Prediction and Update step. Prediction step: where the next state of system is assessed by the values measured before. Update step: where the present state of system is assesses by the values measured at that time. x Prediction: 1 1 k k k m A m (2) 1 1 1 1 k T k k k k Q A P A P x Update: k k k k m H y v k T k k k k R H P H S 1 k T k k k S H P K (3) k k k k v K m m T k k k k k K S K P P Where: x k m and k P : the average predicted value and covariant matrix of separate state at the step k before the measurement value (Priori) respectively. x k m and k P : the average estimated value and covariant matrix of separate state at the step k after the measurement value (Posteriori) respectively. x k v : the new value entered (residual measurement) at the time step k. x k S : the covariant matrix that predicts measurement at the step k. x k K : filter gain of Kalman Filter which needs to find out. According to Särkkä (2006), using Kalman Filter for model on A k and Q k follows the formula: ) exp( k k t F A ' (4) 2009 International Conference on Artificial Intelligence and Computational Intelligence 978-0-7695-3816-7/09 $26.00 © 2009 IEEE DOI 10.1109/AICI.2009.97 335

Upload: thanh-hai

Post on 29-Mar-2017

215 views

Category:

Documents


2 download

TRANSCRIPT

Page 1: [IEEE 2009 International Conference on Artificial Intelligence and Computational Intelligence - Shanghai, China (2009.11.7-2009.11.8)] 2009 International Conference on Artificial Intelligence

Survey of Kalman Filters and Their Application

in Signal Processing

Thanh Binh Ngo Hung Lan Le, Thanh Hai Nguyen

Faculty of Electrical and Electronics, University of Communication and Transport (UCT), Hanoi, Vietnam

E-mail: [email protected], [email protected], [email protected]

Abstract

During the investigation and evaluation of movement,

we are interested in the filter and in smoothing the state of measurement at the previous times, called smoother RTS (Rauch-Tung-Striebel), in order to have a smooth orbit of the movement. Moving objects require an essential time period to speed up to achieve the necessary speed. Thus the obtained measurement and predicted values cannot be beyond the given range in spite of the impact noises. Therefore, in this paper, some limit functions/values are derived to reduce significantly the measurement and evaluation values, to reduce the predicted values before give data to the filter and smoother, in order to remove any unusual values. Consequently, the response of micro-processing systems can be improved. 1. Introduction

This paper presents some tools to develop the application of Kalman filters in digital signal processing, including Filters (02 models: Extended Kalman Filters EKF1, EKF2) and Smoothers (Rauch-Tung-Striebel and Extended Kalman Smoothers). Based on these surveys, we make the evaluation of application in practice. 2. Estimation and Kalman filter

A time-invariant linear model described by continous-time formula as follows:

)()()( tLwtFxdt

tdx (1)

Where : Initial conditions: x(0)~N(m(0),P(0)) F and L : the matrix with features of model. w(t ) : White Noise with the strength of spectrum

is Qc. The classical Kalman Filter that gave a linear model

above was introduced by Rudolph E. Kalman (1960). He

gave a recursive solution (closed form) to estimate time - linearization discrete dynamic systems. Kalman Filter encompasses 2 steps: Prediction and Update step.

Prediction step: where the next state of system is assessed by the values measured before.

Update step: where the present state of system is assesses by the values measured at that time.

Prediction:

11 kkk mAm (2)

1111 kTkkkk QAPAP

Update:

kkkk mHyv

kTkkkk RHPHS

1k

Tkkk SHPK (3)

kkkk vKmm Tkkkkk KSKPP

Where: km and kP : the average predicted value and

covariant matrix of separate state at the step k before the measurement value (Priori) respectively.

km and kP : the average estimated value and covariant matrix of separate state at the step k after the measurement value (Posteriori) respectively.

kv : the new value entered (residual measurement) at the time step k.

kS : the covariant matrix that predicts measurement at the step k.

kK : filter gain of Kalman Filter which needs to find out.

According to Särkkä (2006), using Kalman Filter for model on Ak and Qk follows the formula:

)exp( kk tFA (4)

2009 International Conference on Artificial Intelligence and Computational Intelligence

978-0-7695-3816-7/09 $26.00 © 2009 IEEE

DOI 10.1109/AICI.2009.97

335

Page 2: [IEEE 2009 International Conference on Artificial Intelligence and Computational Intelligence - Shanghai, China (2009.11.7-2009.11.8)] 2009 International Conference on Artificial Intelligence

dtFLLQFQ Tk

TC

t

kk

k

t ))(exp())(exp(0

(5)

Where : kkk ttt 1 : calculated step

It

F

LLQFDC

kT

TC

k

k 00

exp (6)

With : 1kkk DCQ

Conjecture value and assess the status of covariant matrix at different time do not depend on any measurement value. For this reason, we can work out the off-line value before processing the new measurement. However we can build limited function to eliminate unusual signals before entering to Kalman Filter. This reduces unnecessary values calculated unusual errors and increases speed of chip. Thereby, the response ability of system rises. With ground transportation mean, the maximum acceleration is gamax , with 0.1 .

Choose 1max , that means it always meet the

condition : 2max /81.9 sma . To simplify, we work out

with the acceleration on 2 direction on the horizontal plane (Roll axis and Pitch axis). This calculation will be limited by acceleration ( 2

max /81.9 sma ) and velocity (under 400 km/h)

3. Extended Kalman filter

There are many nonlinear systems that the classical calculation of Kalman Filter is not applied in term of evaluating the state of systems. For this reason, the Extended Kalman Filter (EFK) can resolve this problem and Filter Model used EFK as follows:

kkk

kkk

rkxhyqkxfx

),()1,( 11 (7)

Where : n

kx - state of system at step k

mky - measured value at step k

),0(~ 11 kk QNq - process noise ),0(~ kk RNr - measured noise f - nonlinear dynamic function of model h – nonlinear measured vector function of model

In this article, the author introduces EKF1, EKF2 and develops RST, EKS with distributed approximately of

kx state received by observing ky :1 . So Gauss mixed noise given by formula as follows:

),()( :1 kkkkk PmxNyxp (8)

Extended Kalman filter EKF1 Prediction:

)1,( 1 kmfm kk

1111 )1,()1,( kkT

xkkxk QkmFPkmFP (9) Update:

),( kmhyv kkk

kkTxkkxk RkmHPkmHS ),(),(

1),( kkTxkk SkmHPK (10)

kkkk vKmm Tkkkkk KSKPP

Where: )1,( kmFx and ),( kmH x are: Jacobi matrix of f and h function respectively. They described by formula:

mxj

jjjx x

kxfkmF

'',

)1,()]1,([ (11)

mxj

jjjx x

kxhkmH

'',

),()],([ (12)

The difference between KF and EKF1 is only : kA

and kH matrix in KF replaced by Jacobi matrix

)1,( 1 kmF kx and ),( kmH kx in EKF1. Also

conjecture value mean km and residual value of kv is calculated differently in EKF1.

Extended Kalman filter EKF2 Prediction:

})1,({21)1,( 11

)(1 kk

ixx

iikk PkmFtrekmfm (13)

111)'(

11)(

'',

111

})1,()1,({21

)1,()1,(

kkki

xxkki

xxTi

iii

kTxkkkk

QPkmFPkmFtree

kmFPkmFP

Update:

}),({21),( )(

kki

xxi

ikkk PkmHtrekmhyv (14)

kkki

xxkki

xxTi

iii

kTxkkxk

RPkmHPkmHtree

kmHPkmHS

}),(),({21

),(),(

)'()('

',

1),( kkTxkk SkmHPK

kkkk vKmm

Tkkkkk KSKPP

336

Page 3: [IEEE 2009 International Conference on Artificial Intelligence and Computational Intelligence - Shanghai, China (2009.11.7-2009.11.8)] 2009 International Conference on Artificial Intelligence

Where: )1,( kmFx and ),( kmH x are: Jacobi matrix of f and h function as EKF1 in formula (11,12) respectively. )1,()( kmF i

xx and ),()( kmH ixx are

Hessian matrix of if and ih respectively as follow:

mxjj

ijj

ixx xx

kxfkmF'

2

',)( )1,()]1,([ (15)

mxjj

ijj

ixx xx

kxhkmH'

2

',)( ),()],([ (16)

Tie )00100( : unit vector of i Co-ordinate,

that means its value =1 at i position and = 0 at other position.

Extended Kalman Smoother (EKS)

As the same KF and EKF, the difference between EKF and EKS is kA and kH matrix replaced by Jacobi

matrix )1,( 1 kmF kx and ),( kmH kx in EKS.

Conjecture value mean km and its residual value kv are also worked out differently in EKS following the formula:

),(1 kmfm kk

kkT

xkkxk QkmFPkmFP ),(),(1 1

1])[,( kkT

xkk PkmFPC (17)

][ 11 kskkk

sk mmCmm

Tkk

skkk

sk CPPCPP ][ 11

4. Tracking a random sine signal

To prove judgment above, in this simulation we using random sine generated function )sin(),( kkk akxh to replace for the real signals received from the sensor INS. The random sine generated function includes parameters: amplitude, angular velocity and angle changed and mixed random noise. When replacing this sine function by signals received in the actual we will draw the orbit of object

The random sine generated function: )sin(),( kkk akxh

%----------------------------------------------------- % random sine signal % function Y = ekf_test1_h(x,param) f = x(1,:); a = x(3,:); Y = a.*sin(f);

if size(x,1) == 7 Y = Y + x(7,:); …. Y = zeros(1,n); Y_real = feval(h_func,X); Y = Y_real + gauss_rnd(0,R,n);

According to Wiener velocity model, the state vector is presented as follows:

Tkkkk ax (18)

)(100100

)(000000010

)( twtxdt

tdx (19)

Where: k : angle of sine function at the k time,

dtd

k : angular velocity at the time step k,

)(tdtd

ka : amplitude at the time step k

)(tdtda

a

White noise function 2

1

00q

qQc

, with variant

1q and 2q are disordered intensity of velocity vector of noise. In the simulation, chosen q1=0.1, and q2=0.2.

From (4) :

11

10001001

)exp( kkkkk qxt

xtFA (20)

t : step size, chosen 01.0t . In the actual, we can figure out appropriately depending on processing speed of chip.

From (5) :

dtFLLQFQ TK

TC

t

Kk

k

t ))(exp())(exp(0

,

with Gauss random noise ),0(~ 1kk QNq , we calculated:

2

112

12

13

1

00

021

021

31

tq

tqqt

qtqt

Qk

(21)

337

Page 4: [IEEE 2009 International Conference on Artificial Intelligence and Computational Intelligence - Shanghai, China (2009.11.7-2009.11.8)] 2009 International Conference on Artificial Intelligence

With measurement model:

kkkkkk rarkxhy )sin(),( (22) Derivative measurement vector, replacing on formula

EKF and EKS we worked out Jacobi matrix : )sin(0)cos(),( kkkx akmH (23)

%------------------------------------------------------ % Jacobi matrix % function dY = ekf_test1_dh_dx(x, param) f = x(1,:); w = x(2,:); a = x(3,:); dY = [(a.*cos(f))’ zeros(size(f,2),1) (sin(f))’]; and Hesian matrix:

00)cos(000

)cos(0)sin(),(

k

kkk

xx

akmH

(24)

%------------------------------------------------------ %Hesian matrix % function df = ekf_sine_dh2_dx2(x,param) f = x(1); a = x(3); df = zeros(1,3,3); df(1, :, :) = [ -a*sin(f) 0 cos(f);

0 0 0; cos(f) 0 0];

Simulation results:

Case 1: No limited

extended Kalman filter

(EFK1)

Rauch-Tung-Striebel

(RTS)

extended Kalman filter

(EKF2)

Unscented-Extended

(UKS)

Case2: Limited

extended Kalman filter

(EFK1)

Rauch-Tung-Striebel

(RTS)

extended Kalman filter

(EFK2)

Unscented-Extended (UKS)

5. Conclusion:

Programming for micro processing, we should notice

some features: In practice, the results of linear variance and

quadratic can be accepted if transmittal error approximate linear or quadratic functions. Unless, the filter may not be implemented, even the evaluation can be diverged.

In order to change the application there must exist Jacobi and Hesian matrix. In some circumstances, it is not true, for example, in systems having parameters vary suddenly.

In some cases, to figure out Jacobi and Hesian matrix is difficult, it could be failed in calculating derivative and programming on chip. It is difficult to debug also find out these errors with evaluation discrete element of system, specially, when we do not know the required orbit. We need the appropriate steps for each chip. With the micro-processing is not enough powerful, in test1 is a chip 8 bits PSoC CY8C29566 being used, it may spend a lot of time to calculate derivative for the EKF and UKF model.

At the moment, the authors deployed test2 on chips AVR32UC3A0128 and got some satisfactory results such as: stable calculation, locate accurately the real position of object with negligible error.

338

Page 5: [IEEE 2009 International Conference on Artificial Intelligence and Computational Intelligence - Shanghai, China (2009.11.7-2009.11.8)] 2009 International Conference on Artificial Intelligence

6. References [1] Greg Welch, Gary Bishop, “An Introduction to the Kalman Filter”, SIGGRAPH 2001 Journal, Chapel Hill, NC 27599-3175, updated July 24, 2006, on the web. [2] Simon Haykin, Kalman Filtering and Neural Networks, John Wiley & Son, ISBN 0-471-36998, 2001. [3] Jim Ledin, Embedded Control Systems in C/C++: An Introduction for Software Developers Using MATLAB, CMP Books, ISBN 1578201276, 2004. [4] Mohinder S. Grewal, Angus P. Andrews, Kalman Filtering: Theory and Practice Using Matlab, John Wiley & Son, ISBN 0-471-26638-8, 2001 [5] Mohinder S. Grewal, Lawrence R. Weill, Angus P. Andrews, Global Posistioning Systems, Inertial Navigation, and Integration, John Wiley & Son, ISBN-13 978-0-470-04190-1, 2001

339