X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkalman%2Fkalman.5c;h=a91b28d845211cfc191400e721f3de0345b3ec6b;hb=145e5a1ac557c4990e74fb64b005fc68d6941bdc;hp=cfb7abea4ff1435fe8c666433f9455081f014735;hpb=6864e06d88a5b908cffa7c4cd2be8969ff46ce4d;p=fw%2Faltos

diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c
index cfb7abea..a91b28d8 100755
--- a/src/kalman/kalman.5c
+++ b/src/kalman/kalman.5c
@@ -5,7 +5,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -55,6 +56,12 @@ real default_σ_m = 5;
 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;
@@ -87,11 +94,7 @@ parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
  * 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
@@ -140,11 +143,7 @@ parameters_t param_baro(real t, real σ_m, real σ_h) {
  * 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
@@ -191,11 +190,7 @@ parameters_t param_accel(real t, real σ_m, real σ_a) {
  * 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
@@ -236,11 +231,7 @@ parameters_t param_vel(real t) {
  * 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
@@ -467,7 +458,7 @@ void main() {
 					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 ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 			}
 		printf ("\n");
 		exit(0);