```/* nag_kalman_unscented_state (g13ekc) Example Program.
*
* Copyright 2017 Numerical Algorithms Group.
*
* Mark 26.1, 2017.
*/

#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;

#ifdef __cplusplus
extern "C"
{
#endif
static void NAG_CALL f(Integer mx, Integer n, const double *xt,
double *fxt, Nag_Comm *comm, Integer *info);
static void NAG_CALL h(Integer mx, Integer my, Integer n, const double *yt,
double *hyt, Nag_Comm *comm, Integer *info);
#ifdef __cplusplus
}
#endif

static void read_problem_dat(Integer t, Nag_Comm *comm);

int main(void)
{
/* Integer scalar and array declarations */
Integer mx = 3, my = 2;
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;

/* Initialize 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 END2;
}

/* Read in the Cholesky factorization 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 factorization 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 factorization 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("%" NAG_IFMT "%*[^\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_problem_dat(t + 1, &comm);

/* 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(" %3" NAG_IFMT "    ", t + 1);
for (i = 0; i < mx; i++) {
printf(" %10.3f", x[i]);
}
printf("\n");
}

printf("\n");
printf("Estimate of Cholesky Factorization 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:

/* clean up any memory allocated in comm */

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

return (exit_status);
}

static void NAG_CALL 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;
}

static void NAG_CALL 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;
}

static 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("%" NAG_IFMT "%lf%lf%*[^\n] ", &tt, &(pdat->phi_rt),
&(pdat->phi_lt));
if (tt != t) {
/* Sanity check */
printf("Expected to read in data for time point %" NAG_IFMT "\n", t);
printf("Data that was read in was for time point %" NAG_IFMT "\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);
}
}
```