Professional Documents
Culture Documents
A Practical Approach
Paul Zarchan
Numerical Techniques
- Required background
- Introduction to source code
Method of Least Squares
- How to build batch process least squares filter
- Performance and software comparison of different order
filters
Recursive Least Squares Filtering
- How to make batch process filter recursive
- Formulas and properties of various order filters
Column vector
x1
x2
x3
x= .
.
.
xn
Example of vector
5
r= 7
2
Vector addition
x1 y1 x1 + y 1
x2 y2 x2 + y 2
x3 y3 x3 + y 3
x+y = . + . = .
. . .
. . .
xn yn xn + y n
Example
5 -2
r= 7 s= 6
2 4
5 -2 3
r + s = 7 + 6 = 13
2 4 6
5 -2 7
r-s = 7 - 6 = 1
2 4 -2
5
r= 7 rT = 5 7 2
2
2 -6 9 1 11 -5
2 -6 9 1
S + T = 1 5 + -7 2 = -6 7
S= 1 5 T = -7 2
-2 3 5 8 3 11
-2 3 5 8
True BASIC
OPTION NOLET
DIM S(3,2),T(3,2),U(3,2)
S(1,1)=2.
S(1,2)=-6.
S(2,1)=1.
S(2,2)=5.
S(3,1)=-2.
S(3,2)=3.
T(1,1)=9.
T(1,2)=1.
T(2,1)=-7.
T(2,2)=2.
T(3,1)=5.
T(3,2)=8.
MAT U=S+T
MAT PRINT U
END
Transpose of matrix
2 -6 T
ST = 1 5 = 2 1 -2
-6 5 3
-2 3
Because
1 2 3 T 1 2 3
2 5 6 = 2 5 6
3 6 9 3 6 9
SUBROUTINE MATTRN(A,IROW,ICOL,AT)
IMPLICIT REAL*8 (A-H)
IMPLICIT REAL*8 (O-Z)
REAL*8 A(IROW,ICOL),AT(ICOL,IROW)
DO 105 I=1,IROW
DO 105 J=1,ICOL
AT(J,I)=A(I,J)
105 CONTINUE
RETURN
END
Fundamentals of Kalman Filtering: 1 - 18
A Practical Approach
Simple Matrix Operations - 3
A matrix with m rows and n columns can only be multiplied by a
matrix with n rows and q columns
- Multiply each element of the rows of matrix A with
each element of the columns of matrix B
- New matrix has m rows and q columns
Example
-1 6 2 2 -6 -1*2+6*1+2*(-2) -1*(-6)+6*5+2*3 0 42
RS = 3 4 -5 1 5 = 3*2+4*1-5*(-2) 3*(-6)+4*5-5*3 = 20 -13
7 2 8 -2 3 7*2+2*1+8*(-2) 7*(-6)+2*5+8*3 0 -8
Two by two
1 0
0 1
Three by three
1 0 0
0 1 0
0 0 1
AA-1 = I
A -1 = 1 d -b
ad-bc -c a
Numerical example
A = 2 -4
1 3
A-1 = 1 3 4 = 1 3 4 = .3 .4
2*3-(-4)*1 -1 2 10 -1 2 -.1 .2
Check
Coefficient given by
1 = 1 = -1
aei + bfg + cdh - ceg - bdi - afh 1*5*10+2*6*7+3*4*8-3*5*7-2*4*10-1*6*8 3
Check
-2/3 -4/3 1 1 2 3 -2/3-16/3+7 -4/3-20/3+8 -2-8+10
A-1A = -2/3 11/3 -2 4 5 6 = -2/3+44/3-14 -4/3+55/3-16 -2+22-20
1 -2 1 7 8 10 1-8+7 2-10+8 3-12+10
Or
1 0 0
A-1A = 0 1 0
0 0 1
True BASIC
OPTION NOLET
DIM A(3,3),AINV(3,3)
A(1,1)=1.
A(1,2)=2.
A(1,3)=3.
A(2,1)=4.
A(2,2)=5.
A(2,3)=6.
A(3,1)=7.
A(3,2)=8.
A(3,3)=10.
MAT AINV=INV(A)
MAT PRINT AINV
END
Rearranging terms
f(t )
k
t
t
k
Answer
x = sin!t
Therefore
x = -!2 x
Second-order differential equation we want to solve
Initial conditions
x(0) = 0
Obtained from first and second equations at t=0
x(0) = !
-1
-2
0 2 4 6 8 10
Time (Sec)
t
t
k
h
Fundamentals of Kalman Filtering: 1 - 32
A Practical Approach
Using Second-Order Runge-Kutta Integration to
Solve Same Differential Equation
IMPLICIT REAL*8(A-H)
IMPLICIT REAL*8(O-Z)
OPEN(1,STATUS='UNKNOWN',FILE='DATFIL')
W=2.
T=0.
S=0.
X=0.
XD=W Initial conditions
H=.01
WHILE(T<=10.)
S=S+H
XOLD=X
XDOLD=XD
XDD=-W*W*X
X=X+H*XD Second-order Runge-Kutta
XD=XD+H*XDD
T=T+H integration
XDD=-W*W*X
X=.5*(XOLD+X+H*XD)
XD=.5*(XDOLD+XD+H*XDD)
IF(S>=.09999)THEN
S=0.
XTHEORY=SIN(W*T)
WRITE(9,*)T,X,XTHEORY
WRITE(1,*)T,X,XTHEORY
ENDIF
END DO
PAUSE
CLOSE(1)
END
2
Second-Order
Runge-Kutta Integration
-1
Numerical &
Theoretical
-2
0 2 4 6 8 10
Time (Sec)
p(x)dx = 1
-!
Prob(a!x!b) = p(x)dx
a
Distribution function
x
P(x) = p(u)du
-!
m = E(x) = xp(x)dx
-!
Property
E(x1 + x2 + ... + xn ) = E(x1 ) + E(x2 ) + ... + E( xn)
E(x2 ) = x2 p(x)dx
-!
rms = E(x2 )
For zero mean processes the standard deviation and RMS values
are identical
Mean
! b
Variance
3 3 2 (b - a) 2
!2 = E(x2 ) - E2 (x) = b - a - b + a =
3(b - a) 2 12
0.4
0.3
! p(x)
0.2
0.1
0.0
m-4! m-2! m m + 2! m + 4!
x
True BASIC
OPTION NOLET
REM UNSAVE "DATFIL"
OPEN #1:NAME "DATFIL",ACCESS OUTPUT,CREATE NEW, ORGANIZATION TEXT
SET #1: MARGIN 1000
SIGNOISE=1.
FOR I=1 TO 1000
CALL GAUSS(X,SIGNOISE)
PRINT I,X
PRINT #1:I,X
NEXT I
CLOSE #1
END
SUB GAUSS(X,SIG)
LET X=RND+RND+RND+RND+RND+RND-3
LET X=1.414*X*SIG
END SUB
-2
-4
0 200 400 600 800 1000
Number of random numbers
0.3
Calculated
0.2
0.1
Theory
0.0
-6 -4 -2 0 2 4 6
x
0.4
5000 Gaussian
Generated Numbers
Calculated
0.3
0.2
Theory
0.1
0.0
-6 -4 -2 0 2 4 6
x
n
! xi
mean = i=1
n
n
! x2i
mean square = i=1
n-1
n
! (xi - mean)2
i=1
standard deviation =
n-1
Autocorrelation function
!xx (t1 ,t2 ) = E[x(t1 )x(t2 )]
1
x y
1 + sT
4
Theory
-2
-4
Simulation Theory
0 1 2 3 4 5
Time (Sec)
General form
x = Fx + Gu + w
G=0
w=n
T
F= 0 1 w= 0
-3 -2 0
G= 0
1
x = Fx F is time invariant
Is given by
x = sin!t
x = 0 1 x x = Fx + Gu + w
x -!2 0 x
Substitution yields
-1
s -1
( sI - F )-1 =
!2 s
-1 1 s 1
!(s) = ( sI - F ) =
s2 +"2 -"2 s
cos"t sin"t
!(t) = "
-"sin"t cos"t
x(0) = !cos!(0) = !
Since
x(t) = !(t - t0 )x(t0 )
Or
x(t) = sin!t
x(t) = !cos!t
Therefore
0 1 0 1 -!2 0
F2 = =
-!2 0 -!2 0 0 -!2
-!2 0 0 1 0 -!2
F3 = =
0 -!2 -!2 0 !4 0
0 -!2 0 1 !4 0
F4 = =
!4 0 -!2 0 0 !4
!4 0 0 1 0 !4
F5 = =
0 !4 -!2 0 -!6 0
0 !4 0 1 -!6 0
F6 = =
-!6 0 -!2 0 0 -!6
Or
0 1 -#2 0 t2 + 0 -#2 t3 +
!(t) " 1 0 + t+
0 1 -#2 0 0 -#2 2 #4 0 6
# 0 t4
4
0 #4 5
t + -#6 0 t6
+
0 #4 24 -#6 0 120 0 -# 6 720
Combining terms
1- # t +# t -# t t- # t +# t
22 44 66 23 45
Recognizing that
sin!t " !t - ! t + ! t - ...
33 55
3! 5!
We get
cos"t sin"t
!(t) = "
-"sin"t cos"t
Which is the same answer obtained with the Laplace transform method
!
= F!, !(0) = I
! x $ ! 0 1 $ ! x $
# x& = # '( 2 0 & # x &
" % " %" %
$ sin" t '
!(t) = & cos"t " ))
&#" sin"t cos
% "t(
REAL*8 F(2,2),PHI(2,2),PHIOLD(2,2),PHID(2,2)
INTEGER ORDER
OPEN(1,STATUS='UNKNOWN',FILE='DATFIL')
ORDER=2
W=3.
TF=10.
T=0.
S=0.
DO 14 I=1,ORDER
DO 14 J=1,ORDER F and 0
F(I,J)=0.
PHI(I,J)=0.
14 CONTINUE
F(1,2)=1.
F(2,1)=-W*W
PHI(1,1)=1.
PHI(2,2)=1.
H=.01
WHILE(T<=TF) Second-Order
DO 20 I=1,ORDER Runge-Kutta
DO 20 J=1,ORDER
PHIOLD(I,J)=PHI(I,J) Integration
20 CONTINUE
! (t)
0.0
11
0.0
12
-0.1
-0.5
-0.2
-1.0 -0.3
2 4 6 8 10 2 4 6 8 10
Time (s) Time (s)
3 1.0
2
1 0.5
! (t)
! (t)
0
21
0.0
22
-1
-0.5
-2
-3 -1.0
2 4 6 8 10 2 4 6 8 10
Time (s) Time (s)
Fundamentals of Kalman Filtering: 1 - 66
A Practical Approach
Numerical Basics
Summary