```/* nag_kalman_unscented_state (g13ekc) Example Program.
*
* Copyright 2014 Numerical Algorithms Group.
*
* Mark 25, 2014.
*/
/* Pre-processor includes */
#include <stdio.h>
#include <math.h>
#include <nag.h>
#include <nag_stdlib.h>
#include <nagg13.h>
#include <nagx01.h>

#define LY(I,J) ly[(J) * my + (I)]
#define LX(I,J) lx[(J) * mx + (I)]
#define ST(I,J) st[(J) * mx + (I)]
#define XT(I,J) xt[(J) * mx + (I)]
#define FXT(I,J) fxt[(J) * mx + (I)]
#define YT(I,J) yt[(J) * mx + (I)]
#define HYT(I,J) hyt[(J) * my + (I)]

typedef struct g13_problem_data {
double delta, a, r, d;
double phi_rt, phi_lt;
} g13_problem_data;

const Integer mx = 3, my = 2;
void f(Integer mx, Integer n, const double *xt, double *fxt, Nag_Comm *comm,
Integer *info);
void h(Integer mx, Integer my, Integer n, const double *yt, double *hyt,
Nag_Comm *comm, Integer *info);

int main(void)
{
/* Integer scalar and array declarations */
Integer i, ntime, t, j;
Integer exit_status = 0;

/* NAG structures and types */
NagError fail;
Nag_Comm comm;

/* Double scalar and array declarations */
double *lx = 0, *ly = 0, *st = 0, *x = 0, *y = 0;

/* Initialise the error structure */
INIT_FAIL(fail);

printf("nag_kalman_unscented_state (g13ekc) Example Program Results\n\n");

/* Skip heading in data file */
scanf("%*[^\n] ");

if (!(lx = NAG_ALLOC(mx*mx, double)) ||
!(ly = NAG_ALLOC(my*my, double)) ||
!(x = NAG_ALLOC(mx, double)) ||
!(st = NAG_ALLOC(mx*mx, double)) ||
!(y = NAG_ALLOC(my, double))) {
printf("Allocation failure\n");
exit_status = -1;
goto END;
}

/* Read in the cholesky factorisation of the covariance matrix for the
process noise */
for (i = 0; i < mx; i++) {
for (j = 0; j <= i; j++) {
scanf("%lf",&LX(i,j));
}
scanf("%*[^\n] ");
}

/* Read in the cholesky factorisation of the covariance matrix for the
observation noise */
for (i = 0; i < my; i++) {
for (j = 0; j <= i; j++) {
scanf("%lf",&LY(i,j));
}
scanf("%*[^\n] ");
}

/* Read in the initial state vector */
for (i = 0; i < mx; i++) {
scanf("%lf",&x[i]);
}
scanf("%*[^\n] ");

/* Read in the cholesky factorisation of the initial state covariance
matrix */
for (i = 0; i < mx; i++) {
for (j = 0; j <= i; j++) {
scanf("%lf",&ST(i,j));
}
scanf("%*[^\n] ");
}

/* Read in the number of time points to run the system for */
scanf("%ld%*[^\n] ",&ntime);

/* Read in any problem specific data that is constant */

/* Title for first set of output */
printf("   Time  ");
for (i = 0; i < (11*mx- 16)/2; i++) putchar(' ');
printf("Estimate of State\n ");
for (i = 0; i < 7+11*mx; i++) putchar('-');
printf("\n");

/* Loop over each time point */
for (t = 0; t < ntime; t++) {

/* Read in any problem specific data that is time dependent */

/* Read in the observed data for time t */
for (i = 0; i < my; i++) {
scanf("%lf",&y[i]);
}
scanf("%*[^\n] ");

/* Call Unscented Kalman Filter routine (g13ekc) */
nag_kalman_unscented_state(mx,my,y,lx,ly,f,h,x,st,&comm,&fail);
if (fail.code != NE_NOERROR) {
printf("Error from nag_kalman_unscented_state (g13ekc).\n%s\n",
fail.message);
exit_status = 1;
goto END;
}

/* Display the some of the current state estimate */
printf(" %3ld    ",t+1);
for (i = 0; i < mx; i++) {
printf(" %10.3f",x[i]);
}
printf("\n");
}

printf("\n");
printf("Estimate of Cholesky Factorisation of the State\n");
printf("Covariance Matrix at the Last Time Point\n");
for (i = 0; i < mx; i++) {
for (j = 0; j <= i; j++) {
printf(" %10.2e",ST(i,j));
}
printf("\n");
}

END:
NAG_FREE(lx);
NAG_FREE(ly);
NAG_FREE(st);
NAG_FREE(x);
NAG_FREE(y);

/* clean up any memory allocated in comm */

return(exit_status);
}

void f(Integer mx, Integer n, const double *xt, double *fxt, Nag_Comm *comm,
Integer *info) {
double t1, t3;
Integer i;
g13_problem_data *pdat;

/* Cast the void point in comm back to point at the data structure */
pdat = (g13_problem_data *) comm->p;

t1 = 0.5 * pdat->r * (pdat->phi_rt+pdat->phi_lt);
t3 = (pdat->r/pdat->d)*(pdat->phi_rt-pdat->phi_lt);

for (i = 0; i < n; i++) {
FXT(0,i) = XT(0,i) + cos(XT(2,i))*t1;
FXT(1,i) = XT(1,i) + sin(XT(2,i))*t1;
FXT(2,i) = XT(2,i) + t3;
}
/* Set info nonzero to terminate execution for any reason. */
*info = 0;
}

void h(Integer mx, Integer my, Integer n, const double *yt, double *hyt,
Nag_Comm *comm, Integer *info) {
Integer i;
g13_problem_data *pdat;

/* Cast the void point in comm back to point at the data structure */
pdat = (g13_problem_data *) comm->p;

for (i = 0; i < n; i++) {
HYT(0,i) = pdat->delta - YT(0,i)*cos(pdat->a) - YT(1,i)*sin(pdat->a);
HYT(1,i) = YT(2,i) - pdat->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(1,i) < 0.0)
HYT(1,i) += 2 * X01AAC;
}
/* Set info nonzero to terminate execution for any reason. */
*info = 0;
}
void read_problem_dat(Integer t, Nag_Comm *comm) {
/* Read in any data specific to the f and h functions */
Integer tt;
g13_problem_data *pdat;

if (t==0) {
/* Allocate some memory to hold the data */
pdat = NAG_ALLOC(1,g13_problem_data);

/* Read in the data that is constant across all time points */
scanf("%lf%lf%lf%lf%*[^\n] ",&(pdat->r), &(pdat->d), &(pdat->delta),
&(pdat->a));
/* Set the void pointer in comm to point to the data structure */
comm->p = (void *) pdat;

} else if (t > 0) {
/* Cast the void point in comm back to point at the data structure */
pdat = (g13_problem_data *) comm->p;

/* Read in data for time point t */
scanf("%ld%lf%lf%*[^\n] ",&tt, &(pdat->phi_rt), &(pdat->phi_lt));
if (tt!=t) {
/* Sanity check */
printf("Expected to read in data for time point %ld\n",t);
printf("Data that was read in was for time point %ld\n",tt);
}

} else {
/* Clean up */

/* Cast the void point in comm back to point at the data structure */
pdat = (g13_problem_data *) comm->p;

if (pdat)
NAG_FREE(pdat);
}
}
```