diff options
| author | Keith Packard <keithp@keithp.com> | 2013-01-16 15:01:12 -0800 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2013-01-16 15:21:24 -0800 |
| commit | dd60d85d07b881ac03294a8cf607e469f2e69610 (patch) | |
| tree | 255e3d9b4fc65b10c140551d685f84ec6461a138 | |
| parent | 994ff76a064dcbd3113db771cd9cd9591fd68dea (diff) | |
altos: Correct model error covariance matrix
Finally found a couple of decent references on how to set the model
(process) error covariance matrix. The current process matrix turns
out to be correct for a continuous kalman filter (which isn't
realizable, of course). For a discrete filter, the error in modeled
acceleration (we model it as a constant) needs to be propogated to the
speed and position portions of the matrix.
The correct matrix is seen in this paper:
On Reduced-Order Kalman Filters For GPS Position Filtering
J. Shima
6/2/2001
This references an older paper which is supposed to describe the
derivation of the matrix:
Singer, R.A., “Estimating Optimal Tracking Filter Performance for Manned Maneuvering Targets,”
IEEE Transactions of Aerospace and Electronic Systems, AES-5, July 1970, pp. 473-483.
This change has a minor effect on the computed correction
coefficients; it should respond more reasonably to acceleration
changes now.
Signed-off-by: Keith Packard <keithp@keithp.com>
| -rwxr-xr-x | src/kalman/kalman.5c | 30 |
1 files changed, 10 insertions, 20 deletions
diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c index cfb7abea..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 |