```!   G13EKF Example Program Text
!   Mark 27.1 Release. NAG Copyright 2020.

Module g13ekfe_mod

!     G13EKF Example Program Module:
!     User-defined Routines

!     .. Use Statements ..
Use nag_library, Only: nag_wp
!     .. Implicit None Statement ..
Implicit None
!     .. Accessibility Statements ..
Private
!     .. Parameters ..
Integer, Parameter, Public       :: mx = 3, my = 2, nin = 5, nout = 6
Contains
Subroutine f(mx,n,xt,fxt,iuser,ruser,info)

!       .. Scalar Arguments ..
Integer, Intent (Inout)        :: info
Integer, Intent (In)           :: mx, n
!       .. Array Arguments ..
Real (Kind=nag_wp), Intent (Out) :: fxt(mx,n)
Real (Kind=nag_wp), Intent (Inout) :: ruser(*)
Real (Kind=nag_wp), Intent (In) :: xt(mx,n)
Integer, Intent (Inout)        :: iuser(*)
!       .. Local Scalars ..
Real (Kind=nag_wp)             :: d, phi_lt, phi_rt, r, t1, t3
Integer                        :: i
!       .. Intrinsic Procedures ..
Intrinsic                      :: cos, sin
!       .. Executable Statements ..
Continue

r = ruser(3)
d = ruser(4)
phi_rt = ruser(5)
phi_lt = ruser(6)

t1 = 0.5_nag_wp*r*(phi_rt+phi_lt)
t3 = (r/d)*(phi_rt-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

!       Set info nonzero to terminate execution for any reason.
info = 0

Return
End Subroutine f
Subroutine h(mx,my,n,yt,hyt,iuser,ruser,info)

!       .. Use Statements ..
Use nag_library, Only: x01aaf
!       .. Scalar Arguments ..
Integer, Intent (Inout)        :: info
Integer, Intent (In)           :: mx, my, n
!       .. Array Arguments ..
Real (Kind=nag_wp), Intent (Out) :: hyt(my,n)
Real (Kind=nag_wp), Intent (Inout) :: ruser(*)
Real (Kind=nag_wp), Intent (In) :: yt(mx,n)
Integer, Intent (Inout)        :: iuser(*)
!       .. Local Scalars ..
Real (Kind=nag_wp)             :: a, delta, tmp
Integer                        :: i
!       .. Intrinsic Procedures ..
Intrinsic                      :: cos, sin
!       .. Executable Statements ..
Continue

delta = ruser(1)
a = ruser(2)

Do i = 1, n
hyt(1,i) = delta - yt(1,i)*cos(a) - yt(2,i)*sin(a)
hyt(2,i) = yt(3,i) - 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

!       Set info nonzero to terminate execution for any reason.
info = 0

Return
End Subroutine h
!       Read in any data specific to the F and H subroutines

!       .. Scalar Arguments ..
Integer, Intent (In)           :: t
!       .. Array Arguments ..
Real (Kind=nag_wp), Allocatable, Intent (Inout) :: ruser(:)
Integer, Allocatable, Intent (Inout) :: iuser(:)
!       .. Local Scalars ..
Real (Kind=nag_wp)             :: a, d, delta, phi_lt, phi_rt, r
Integer                        :: tt
!       .. Executable Statements ..
Continue

If (t==0) Then
!         Allocate the arrays to hold the data
Allocate (ruser(6),iuser(0))

!         Read in the data that is constant across all time points
Read (nin,*) r, d, delta, a

!         Store the data in RUSER
ruser(1) = delta
ruser(2) = a
ruser(3) = r
ruser(4) = d

Else
!         Read in data for time point t
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)
Else
End If

!         Store the data in RUSER
ruser(5) = phi_rt
ruser(6) = phi_lt

End If
End Module g13ekfe_mod

Program g13ekfe

!     .. Use Statements ..
Use g13ekfe_mod, Only: f, h, mx, my, nin, nout, read_problem_data
Use nag_library, Only: g13ekf, nag_wp
!     .. Implicit None Statement ..
Implicit None
!     .. Local Scalars ..
Integer                          :: i, ifail, ntime, t
!     .. Local Arrays ..
Real (Kind=nag_wp), Allocatable  :: lx(:,:), ly(:,:), ruser(:), st(:,:), &
x(:), y(:)
Integer, Allocatable             :: iuser(:)
!     .. Intrinsic Procedures ..
Intrinsic                        :: repeat
!     .. Executable Statements ..
Write (nout,*) 'G13EKF Example Program Results'
Write (nout,*)

!     Skip heading in data file

!     Allocate arrays
Allocate (lx(mx,mx),ly(my,my),x(mx),st(mx,mx),y(my))

!     Read in the Cholesky factorization of the covariance matrix for the
!     process noise
Do i = 1, mx
End Do

!     Read in the Cholesky factorization of the covariance matrix for the
!     observation noise
Do i = 1, my
End Do

!     Read in the initial state vector

!     Read in the Cholesky factorization of the initial state covariance
!     matrix
Do i = 1, mx
End Do

!     Read in the number of time points to run the system for

!     Read in any problem specific data that is constant
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
Do t = 1, ntime

!       Read in any problem specific data that is time dependent
Go To 100
End If

!       Read in the observed data for time t

!       Call Unscented Kalman Filter routine
ifail = 0
Call g13ekf(mx,my,y,lx,ly,f,h,x,st,iuser,ruser,ifail)

!       Display the current state estimates
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 g13ekfe
```