real default_σ_h = 20;
real default_σ_a = 2;
+real[3,3] model_error(t, Φ) = multiply_mat_val ((real[3,3]) {
+ { t**5 / 20, t**4 / 8, t**3 / 6 },
+ { t**4 / 8, t**3 / 3, t**2 / 2 },
+ { t**3 / 6, t**2 / 2, t }
+ }, Φ);
+
parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
if (σ_m == 0)
σ_m = default_σ_m;
* Model error covariance. The only inaccuracy in the
* model is the assumption that acceleration is constant
*/
- .q = (real[3,3]) {
- { 0, 0, 0 },
- { 0, 0, 0 },
- {.0, 0, σ_m**2 },
- },
+ .q = model_error (t, σ_m**2),
/*
* Measurement error covariance
* Our sensors are independent, so
* Model error covariance. The only inaccuracy in the
* model is the assumption that acceleration is constant
*/
- .q = (real[3,3]) {
- { 0, 0, 0 },
- { 0, 0, 0 },
- {.0, 0, σ_m**2 },
- },
+ .q = model_error (t, σ_m**2),
/*
* Measurement error covariance
* Our sensors are independent, so
* Model error covariance. The only inaccuracy in the
* model is the assumption that acceleration is constant
*/
- .q = (real[3,3]) {
- { 0, 0, 0 },
- { 0, 0, 0 },
- {.0, 0, σ_m**2 },
- },
+ .q = model_error (t, σ_m**2),
/*
* Measurement error covariance
* Our sensors are independent, so
* Model error covariance. The only inaccuracy in the
* model is the assumption that acceleration is constant
*/
- .q = (real[3,3]) {
- { 0, 0, 0 },
- { 0, 0, 0 },
- {.0, 0, σ_m**2 },
- },
+ .q = model_error (t, σ_m**2),
/*
* Measurement error covariance
* Our sensors are independent, so
mat_t k = converge(param);
int[] d = dims(k);
int time_inc = floor(1/time_step + 0.5);
- if (d[1] == 2) {
- for (int i = 0; i < d[0]; i++)
- for (int j = 0; j < d[1]; j++)
- printf ("#define %s_K%d%d_%d to_fix16(%12.10f)\n",
- prefix, i, j, time_inc, k[i,j]);
- } else {
- for (int i = 0; i < d[0]; i++) {
- printf ("#define %s_K%d_%d to_fix16(%12.10f)\n",
- prefix, i, time_inc, k[i,0]);
+ for (int i = 0; i < d[0]; i++)
+ for (int j = 0; j < d[1]; j++) {
+ string name;
+ if (d[1] == 1)
+ name = sprintf("%s_K%d_%d", prefix, i, time_inc);
+ else
+ name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
+ printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
}
- }
printf ("\n");
- return;
+ exit(0);
}
string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] };