/* nag_tsa_kalman_unscented_state_revcom (g13ejc) Example Program.
*
* Copyright 2021 Numerical Algorithms Group.
*
* Mark 27.3, 2021.
*/
/* Pre-processor includes */
#include <math.h>
#include <nag.h>
#include <stdio.h>
#define LY(I, J) ly[(J)*pdly + (I)]
#define LX(I, J) lx[(J)*pdlx + (I)]
#define ST(I, J) st[(J)*pdst + (I)]
#define XT(I, J) xt[(J)*pdxt + (I)]
#define FXT(I, J) fxt[(J)*pdfxt + (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 n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
g13_problem_data dat);
void h(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
g13_problem_data dat);
void read_problem_dat(Integer t, g13_problem_data *dat);
int main(void) {
/* Integer scalar and array declarations */
Integer i, irevcm, pdfxt, pdlx, pdly, pdst, pdxt, licomm, lrcomm, lropt, n,
ntime, t, j;
Integer *icomm = 0;
Integer exit_status = 0;
/* NAG structures and types */
NagError fail;
/* Double scalar and array declarations */
double *fxt = 0, *lx = 0, *ly = 0, *rcomm = 0, *ropt = 0, *st = 0, *x = 0,
*xt = 0, *y = 0;
/* Other structures */
g13_problem_data dat;
/* Initialize the error structure */
INIT_FAIL(fail);
printf("nag_tsa_kalman_unscented_state_revcom (g13ejc) "
"Example Program Results\n\n");
/* Skip heading in data file */
scanf("%*[^\n] ");
/* Using default optional arguments */
lropt = 0;
/* Allocate arrays */
n = 2 * mx + 1;
if (lropt >= 1 && fabs(ropt[0] - 2.0) <= 0.0) {
n += 2 * mx;
}
pdlx = pdst = pdxt = mx;
pdly = my;
pdfxt = (mx > my) ? mx : my;
licomm = 30;
lrcomm = 30 + my + mx * my + 2 * ((mx > my) ? mx : my);
if (!(lx = NAG_ALLOC(pdlx * mx, double)) ||
!(ly = NAG_ALLOC(pdly * my, double)) || !(x = NAG_ALLOC(mx, double)) ||
!(st = NAG_ALLOC(pdst * mx, double)) ||
!(xt = NAG_ALLOC(pdxt * (my > n ? my : n), double)) ||
!(fxt = NAG_ALLOC(pdfxt * (n + (mx > my ? mx : my)), double)) ||
!(icomm = NAG_ALLOC(licomm, Integer)) ||
!(rcomm = NAG_ALLOC(lrcomm, double)) || !(y = NAG_ALLOC(my, double))) {
printf("Allocation failure\n");
exit_status = -1;
goto END;
}
/* 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 */
read_problem_dat(0, &dat);
/* 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 */
irevcm = 0;
for (t = 0; t < ntime; t++) {
/* Read in any problem specific data that is time dependent */
read_problem_dat(t + 1, &dat);
/* 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 (g13ejc) */
do {
nag_tsa_kalman_unscented_state_revcom(
&irevcm, mx, my, y, lx, pdlx, ly, pdly, x, st, pdst, &n, xt, pdxt,
fxt, pdfxt, ropt, lropt, icomm, licomm, rcomm, lrcomm, &fail);
switch (irevcm) {
case 1:
/* Evaluate F(X) */
f(n, xt, pdxt, fxt, pdfxt, dat);
break;
case 2:
/* Evaluate H(X) */
h(n, xt, pdxt, fxt, pdfxt, dat);
break;
default:
/* irevcm = 3, finished */
if (fail.code != NE_NOERROR) {
printf(
"Error from nag_tsa_kalman_unscented_state_revcom (g13ejc).\n%s\n",
fail.message);
exit_status = 1;
goto END;
}
break;
}
} while (irevcm != 3);
/* Display the current state estimates */
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:
NAG_FREE(icomm);
NAG_FREE(fxt);
NAG_FREE(lx);
NAG_FREE(ly);
NAG_FREE(rcomm);
NAG_FREE(ropt);
NAG_FREE(st);
NAG_FREE(x);
NAG_FREE(xt);
NAG_FREE(y);
return (exit_status);
}
void f(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
g13_problem_data dat) {
double t1, t3;
Integer i;
t1 = 0.5 * dat.r * (dat.phi_rt + dat.phi_lt);
t3 = (dat.r / dat.d) * (dat.phi_rt - dat.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;
}
}
void h(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
g13_problem_data dat) {
Integer i;
for (i = 0; i < n; i++) {
FXT(0, i) = dat.delta - XT(0, i) * cos(dat.a) - XT(1, i) * sin(dat.a);
FXT(1, i) = XT(2, 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 (FXT(1, i) < 0.0)
FXT(1, i) += 2 * X01AAC;
}
}
void read_problem_dat(Integer t, g13_problem_data *dat) {
/* Read in any data specific to the f and h functions */
Integer tt;
if (t == 0) {
/* Read in the data that is constant across all time points */
scanf("%lf%lf%lf%lf%*[^\n] ", &(dat->r), &(dat->d), &(dat->delta),
&(dat->a));
} else {
/* Read in data for time point t */
scanf("%" NAG_IFMT "%lf%lf%*[^\n] ", &tt, &(dat->phi_rt), &(dat->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);
}
}
}