/* nag_kalman_sqrt_filt_info_var (g13ecc) Example Program. * * Copyright 1993 Numerical Algorithms Group * * Mark 3, 1993 * Mark 8 revised, 2004. */ #include #include #include #include #define AINV(I, J) ainv[(I) *tdainv + J] #define QINV(I, J) qinv[(I) *tdqinv + J] #define RINV(I, J) rinv[(I) *tdrinv + J] #define T(I, J) t[(I) *tdt + J] #define B(I, J) b[(I) *tdb + J] #define C(I, J) c[(I) *tdc + J] int main(void) { Integer exit_status = 0, i, istep, j, m, n, p, tdainv, tdb, tdc, tdqinv, tdrinv; Integer tdt; Nag_ab_input inp_ab; double *ainv = 0, *b = 0, *c = 0, *qinv = 0, *rinv = 0, *rinvy = 0; double *t = 0, tol, *x = 0, *z = 0; NagError fail; INIT_FAIL(fail); printf( "nag_kalman_sqrt_filt_info_var (g13ecc) Example Program Results\n"); /* Skip the heading in the data file */ scanf("%*[^\n]"); scanf("%ld%ld%ld%lf", &n, &m, &p, &tol); if (n >= 1 && m >= 1 && p >= 1) { if (!(ainv = NAG_ALLOC(n*n, double)) || !(qinv = NAG_ALLOC(m*m, double)) || !(rinv = NAG_ALLOC(p*p, double)) || !(t = NAG_ALLOC(n*n, double)) || !(b = NAG_ALLOC(n*m, double)) || !(c = NAG_ALLOC(p*n, double)) || !(x = NAG_ALLOC(n, double)) || !(z = NAG_ALLOC(m, double)) || !(rinvy = NAG_ALLOC(p, double))) { printf("Allocation failure\n"); exit_status = -1; goto END; } tdainv = n; tdqinv = m; tdrinv = p; tdt = n; tdb = m; tdc = n; } else { printf("Invalid n or m or p.\n"); exit_status = 1; return exit_status; } inp_ab = Nag_ab_prod; /* Read data */ for (i = 0; i < n; ++i) for (j = 0; j < n; ++j) scanf("%lf", &AINV(i, j)); for (i = 0; i < p; ++i) for (j = 0; j < n; ++j) scanf("%lf", &C(i, j)); if (rinv) for (i = 0; i < p; ++i) for (j = 0; j < p; ++j) scanf("%lf", &RINV(i, j)); for (i = 0; i < n; ++i) for (j = 0; j < m; ++j) scanf("%lf", &B(i, j)); for (i = 0; i < m; ++i) for (j = 0; j < m; ++j) scanf("%lf", &QINV(i, j)); for (i = 0; i < n; ++i) for (j = 0; j < n; ++j) scanf("%lf", &T(i, j)); for (j = 0; j < m; ++j) scanf("%lf", &z[j]); for (j = 0; j < n; ++j) scanf("%lf", &x[j]); for (j = 0; j < p; ++j) scanf("%lf", &rinvy[j]); /* Perform three iterations of the (Kalman) filter recursion (in square root information form). */ for (istep = 1; istep <= 3; ++istep) /* nag_kalman_sqrt_filt_info_var (g13ecc). * One iteration step of the time-varying Kalman filter * recursion using the square root information * implementation */ nag_kalman_sqrt_filt_info_var(n, m, p, inp_ab, t, tdt, ainv, tdainv, b, tdb, rinv, tdrinv, c, tdc, qinv, tdqinv, x, rinvy, z, tol, &fail); if (fail.code != NE_NOERROR) { printf( "Error from nag_kalman_sqrt_filt_info_var (g13ecc).\n%s\n", fail.message); exit_status = 1; goto END; } printf("\nThe inverse of the square root of the state covariance " "matrix is\n\n"); for (i = 0; i < n; ++i) { for (j = 0; j < n; ++j) printf("%8.4f ", T(i, j)); printf("\n"); } printf("\nThe components of the estimated filtered state are\n\n"); printf("k x(k) \n"); for (i = 0; i < n; ++i) { printf("%ld ", i); printf(" %8.4f \n", x[i]); } END: if (ainv) NAG_FREE(ainv); if (qinv) NAG_FREE(qinv); if (rinv) NAG_FREE(rinv); if (t) NAG_FREE(t); if (b) NAG_FREE(b); if (c) NAG_FREE(c); if (x) NAG_FREE(x); if (z) NAG_FREE(z); if (rinvy) NAG_FREE(rinvy); return exit_status; }