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>
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;
* 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
* 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
* 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
* 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