X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkalman%2Fkalman.5c;h=46fa8e1fdefecbcd9dad65a9e1208c4bb2df306a;hp=f734718413f8233c69cd5a58574a6c76c61a8756;hb=dd60d85d07b881ac03294a8cf607e469f2e69610;hpb=20427ae4965f756aac0cedc5179a1c45b9a781f2

diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c
index f7347184..46fa8e1f 100755
--- a/src/kalman/kalman.5c
+++ b/src/kalman/kalman.5c
@@ -55,6 +55,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 +93,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 +142,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 +189,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 +230,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
@@ -460,19 +450,17 @@ void main() {
 		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] };