! G13EJF Example Program Text
! Mark 26.1 Release. NAG Copyright 2016.
Module g13ejfe_mod
! G13EJF Example Program Module:
! User-defined Routines
! .. Use Statements ..
Use nag_library, Only: nag_wp
! .. Implicit None Statement ..
Implicit None
! .. Accessibility Statements ..
Private
Public :: f, h, read_problem_data
! .. Parameters ..
Integer, Parameter, Public :: mx = 3, my = 2, nin = 5, nout = 6
! .. Derived Type Definitions ..
Type, Public :: g13ej_problem_data
Real (Kind=nag_wp) :: delta, a, r, d
Real (Kind=nag_wp) :: phi_rt, phi_lt
End Type g13ej_problem_data
Contains
Subroutine f(n,xt,fxt,dat)
! .. Scalar Arguments ..
Type (g13ej_problem_data), Intent (In) :: dat
Integer, Intent (In) :: n
! .. Array Arguments ..
Real (Kind=nag_wp), Intent (Out) :: fxt(:,:)
Real (Kind=nag_wp), Intent (In) :: xt(:,:)
! .. Local Scalars ..
Real (Kind=nag_wp) :: t1, t3
Integer :: i
! .. Intrinsic Procedures ..
Intrinsic :: cos, sin
! .. Executable Statements ..
Continue
t1 = 0.5_nag_wp*dat%r*(dat%phi_rt+dat%phi_lt)
t3 = (dat%r/dat%d)*(dat%phi_rt-dat%phi_lt)
Do i = 1, n
fxt(1,i) = xt(1,i) + cos(xt(3,i))*t1
fxt(2,i) = xt(2,i) + sin(xt(3,i))*t1
fxt(3,i) = xt(3,i) + t3
End Do
Return
End Subroutine f
Subroutine h(n,yt,hyt,dat)
! .. Use Statements ..
Use nag_library, Only: x01aaf
! .. Scalar Arguments ..
Type (g13ej_problem_data), Intent (In) :: dat
Integer, Intent (In) :: n
! .. Array Arguments ..
Real (Kind=nag_wp), Intent (Out) :: hyt(:,:)
Real (Kind=nag_wp), Intent (In) :: yt(:,:)
! .. Local Scalars ..
Real (Kind=nag_wp) :: tmp
Integer :: i
! .. Intrinsic Procedures ..
Intrinsic :: cos, sin
! .. Executable Statements ..
Continue
Do i = 1, n
hyt(1,i) = dat%delta - yt(1,i)*cos(dat%a) - yt(2,i)*sin(dat%a)
hyt(2,i) = yt(3,i) - dat%a
! Make sure that the theta is in the same range as the observed
! data, which in this case is [0, 2*pi)
If (hyt(2,i)<0.0_nag_wp) Then
hyt(2,i) = hyt(2,i) + 2*x01aaf(tmp)
End If
End Do
Return
End Subroutine h
Subroutine read_problem_data(t,dat,read_ok)
! Read in any data specific to the F and H subroutines
! .. Scalar Arguments ..
Type (g13ej_problem_data), Intent (Inout) :: dat
Integer, Intent (In) :: t
Logical, Intent (Out) :: read_ok
! .. Local Scalars ..
Integer :: tt
! .. Executable Statements ..
Continue
If (t==0) Then
! Read in the data that is constant across all time points
Read (nin,*) dat%r, dat%d, dat%delta, dat%a
read_ok = .True.
Else
! Read in data for time point t
Read (nin,*) tt, dat%phi_rt, dat%phi_lt
If (tt/=t) Then
! Sanity check
Write (nout,99999) 'Expected to read in data for time point ', t
Write (nout,99999) 'Data that was read in was for time point ', tt
99999 Format (A,E22.15)
read_ok = .False.
Else
read_ok = .True.
End If
End If
End Subroutine read_problem_data
End Module g13ejfe_mod
Program g13ejfe
! .. Use Statements ..
Use g13ejfe_mod, Only: f, g13ej_problem_data, h, mx, my, nin, nout, &
read_problem_data
Use nag_library, Only: g13ejf, nag_wp
! .. Implicit None Statement ..
Implicit None
! .. Local Scalars ..
Type (g13ej_problem_data) :: dat
Integer :: i, ifail, irevcm, ldfxt, ldlx, ldly, &
ldst, ldxt, licomm, lrcomm, lropt, &
n, ntime, t
Logical :: read_ok
! .. Local Arrays ..
Real (Kind=nag_wp), Allocatable :: fxt(:,:), lx(:,:), ly(:,:), &
rcomm(:), ropt(:), st(:,:), x(:), &
xt(:,:), y(:)
Integer, Allocatable :: icomm(:)
! .. Intrinsic Procedures ..
Intrinsic :: abs, max, repeat
! .. Executable Statements ..
Write (nout,*) 'G13EJF Example Program Results'
Write (nout,*)
! Skip heading in data file
Read (nin,*)
! Using default optional arguments
lropt = 0
Allocate (ropt(lropt))
! Allocate arrays
n = 2*mx + 1
If (lropt>=1) Then
If (abs(ropt(1)-2.0_nag_wp)<=0.0_nag_wp) Then
n = n + 2*mx
End If
End If
ldlx = mx
ldly = my
ldst = mx
ldxt = mx
ldfxt = max(mx,my)
licomm = 30
lrcomm = 30 + my + mx*my + 2*max(mx,my)
Allocate (lx(ldlx,mx),ly(ldly,my),x(mx),st(ldst,mx),xt(ldxt,max(my, &
n)),fxt(ldfxt,n+max(mx,my)),icomm(licomm),rcomm(lrcomm),y(my))
! Read in the Cholesky factorization of the covariance matrix for the
! process noise
Do i = 1, mx
Read (nin,*) lx(i,1:i)
End Do
! Read in the Cholesky factorization of the covariance matrix for the
! observation noise
Do i = 1, my
Read (nin,*) ly(i,1:i)
End Do
! Read in the initial state vector
Read (nin,*) x(1:mx)
! Read in the Cholesky factorization of the initial state covariance
! matrix
Do i = 1, mx
Read (nin,*) st(i,1:i)
End Do
! Read in the number of time points to run the system for
Read (nin,*) ntime
! Read in any problem specific data that is constant
Call read_problem_data(0,dat,read_ok)
If (.Not. read_ok) Then
Go To 100
End If
! Title for first set of output
Write (nout,*) ' Time ', repeat(' ',(11*mx-16)/2), 'Estimate of State'
Write (nout,*) repeat('-',7+11*mx)
! Loop over each time point
irevcm = 0
Do t = 1, ntime
! Read in any problem specific data that is time dependent
Call read_problem_data(t,dat,read_ok)
If (.Not. read_ok) Then
Go To 100
End If
! Read in the observed data for time t
Read (nin,*) y(1:my)
! Call Unscented Kalman Filter routine
ukf_lp: Do
ifail = 0
Call g13ejf(irevcm,mx,my,y,lx,ldlx,ly,ldly,x,st,ldst,n,xt,ldxt,fxt, &
ldfxt,ropt,lropt,icomm,licomm,rcomm,lrcomm,ifail)
Select Case (irevcm)
Case (1)
! Evaluate F(X)
Call f(n,xt,fxt,dat)
Case (2)
! Evaluate H(X)
Call h(n,xt,fxt,dat)
Case Default
! IREVCM = 3, finished
Exit ukf_lp
End Select
End Do ukf_lp
! Display the some of the current state estimate
Write (nout,99999) t, x(1:mx)
End Do
Write (nout,*)
Write (nout,*) 'Estimate of Cholesky Factorization of the State'
Write (nout,*) 'Covariance Matrix at the Last Time Point'
Do i = 1, mx
Write (nout,99998) st(i,1:i)
End Do
100 Continue
99999 Format (1X,I3,4X,10(1X,F10.3))
99998 Format (10(1X,E10.3))
End Program g13ejfe