projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
altos/telegps-v2.0: git ignore make results
[fw/altos]
/
src
/
kalman
/
kalman.5c
diff --git
a/src/kalman/kalman.5c
b/src/kalman/kalman.5c
index cfb7abea4ff1435fe8c666433f9455081f014735..a91b28d845211cfc191400e721f3de0345b3ec6b 100755
(executable)
--- 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
*
* 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
*
* 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 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;
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
*/
* 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
/*
* 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
*/
* 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
/*
* 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
*/
* 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
/*
* 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
*/
* 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
/*
* 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);
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_fix
32
(%12.10f)\n", name, k[i,j]);
+ printf ("#define %s to_fix
_k
(%12.10f)\n", name, k[i,j]);
}
printf ("\n");
exit(0);
}
printf ("\n");
exit(0);