[ieee 2009 international conference on artificial intelligence and computational intelligence -...
TRANSCRIPT
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
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
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
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
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