From dd60d85d07b881ac03294a8cf607e469f2e69610 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 16 Jan 2013 15:01:12 -0800
Subject: [PATCH] altos: Correct model error covariance matrix
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

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>
---
 src/kalman/kalman.5c | 30 ++++++++++--------------------
 1 file 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
-- 
2.30.2