LQG docs and scope

golems · Apr 6, 2015 · 42b0646 · 42b0646
1 parent 5c71c6f
commit 42b0646
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 25 deletions.
diff --git a/include/reflex/lqg.h b/include/reflex/lqg.h
@@ -91,14 +91,15 @@ void rfx_lqg_kf_correct_cov
 /* The next few typedefs define functions for computing state updates,
  * expected measurements, and linearizing the system */
 
-/** Function to compute process update and linearization.
+/**
+ * Function to compute process update and linearization.
  *
  * \f[ x \approx F x \f]
  *
- * @param cx context struct
- * @param x state vector
- * @param u input vector
- * @param F linearized process model
+ * @param[in,out] cx context struct
+ * @param[in,out] x state vector
+ * @param[in]     u input vector
+ * @param[out]    F linearized process model
  *
  * @pre
  *  - x contains the prior state estimate
@@ -108,7 +109,8 @@ void rfx_lqg_kf_correct_cov
  */
 typedef int (*rfx_lqg_ekf_process_fun)( void *cx, double *x, const double *u, double *F );
 
-/** Function to compute innovation and linearization.
+/**
+ * Function to compute innovation and linearization.
  *
  * \f[ y = h(x) \f]
  *
@@ -117,7 +119,7 @@ typedef int (*rfx_lqg_ekf_process_fun)( void *cx, double *x, const double *u, do
  * @param cx context struct
  * @param x state vector
  * @param y predicted measurement
- * @param F linearized measurement model
+ * @param H linearized measurement model
  *
  * @post
  *  - y contains the predicted measurement for state x

diff --git a/src/lqg/lqg.c b/src/lqg/lqg.c
@@ -177,26 +177,30 @@ int rfx_lqg_ekf_correct
 {
     int i;
 
-    double y[n_z];
     double H[n_z*n_x];
-    i = measure(cx, x, y, H);
-    if(i) return i;
-
     double K[n_x*n_z];
-    i = rfx_lqg_kf_correct_gain( n_x, n_z, H, P, W, K );
-    if(i) return i;
-
-    i = innovate(cx, x, z, y);
-    if(i) return i;
-
-    double Ky[n_x];
-    cblas_dgemv( CblasColMajor, CblasNoTrans,
-                 (int)n_x, (int)n_z,
-                 1.0, K, (int)n_x,
-                 y, 1,
-                 0.0, Ky, 1 );
-
-    i = update(cx, x, Ky);
+    {
+        double y[n_z];
+        i = measure(cx, x, y, H);
+        if(i) return i;
+
+        i = rfx_lqg_kf_correct_gain( n_x, n_z, H, P, W, K );
+        if(i) return i;
+
+        i = innovate(cx, x, z, y);
+        if(i) return i;
+
+        {
+            double Ky[n_x];
+            cblas_dgemv( CblasColMajor, CblasNoTrans,
+                         (int)n_x, (int)n_z,
+                         1.0, K, (int)n_x,
+                         y, 1,
+                         0.0, Ky, 1 );
+
+            i = update(cx, x, Ky);
+        }
+    }
 
     rfx_lqg_kf_correct_cov( n_x, n_z, H, P, K );