John Fitzpatrick
test
Diff: kalman.c
- Revision:
- 1:270647f1e086
- Parent:
- 0:8a460b0d6f09
diff -r 8a460b0d6f09 -r 270647f1e086 kalman.c
--- a/kalman.c Wed Aug 15 22:17:07 2012 +0000
+++ b/kalman.c Sun Mar 28 20:19:54 2021 +0000
@@ -9,39 +9,30 @@
/********************************************************************
* Function Name: kalman_init *
* Return Value: none *
-* Parameters: struct filter, dt, R_angle, Q_gyro, Q_angle *
+* Parameters: struct filter, R_angle, Q_gyro, Q_angle *
* Description: Initialize the kalman Filter parameters. *
********************************************************************/
-void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // double dt,
+void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // Initialization
{
- // Initialize the two states, the angle and the gyro bias. As a
- // byproduct of computing the angle, we also have an unbiased
- // angular rate available.
+ // Initialize the three states, the angle, gyro bias and angular rate
filter->x_bias = 0.0;
filter->x_rate = 0.0;
filter->x_angle = 0.0;
-
- // Initialize the delta in seconds between gyro samples.
- //filter->dt = dt;
-
- // Initialize the measurement noise covariance matrix values.
- // In this case, R is a 1x1 matrix tha represents expected
- // jitter from the accelerometer.
+ // Initialize measurement noise covariance, a 1x1 matrix representing expected accelerometer jitter
+ // R_angel is set to 0.59
filter->R_angle = R_angle;
- // Initialize the process noise covariance matrix values.
- // In this case, Q indicates how much we trust the acceleromter
- // relative to the gyros.
- // Q_gyro set to 0.003 and Q_angle set to 0.001.
+ // Initialize process noise covariance matrix, trust of acceleromter vs trust of gyro
+ // Q_gyro set to 0.002 and Q_angle set to 0.001.
filter->Q_gyro = Q_gyro;
filter->Q_angle = Q_angle;
// Initialize covariance of estimate state. This is updated
// at every time step to determine how well the sensors are
- // tracking the actual state.
+ // tracking the actual state. Initially an Identity matrix
filter->P_00 = 1.0;
filter->P_01 = 0.0;
@@ -50,29 +41,27 @@
}
/********************************************************************
-* Function Name: kalman_predict *
+* Function Name: kalman_predict *
* Return Value: none *
-* Parameters: struct filter, measured gyroscope value *
-* Description: Called every dt(Timer 1 overflow with a biased *
-* gyro. Also updates the current rate and angle *
-* estimate). *
+* Parameters: struct filter, measured gyro value *
+* Description: Called at dt, updates current rate and angle *
********************************************************************/
void kalman_predict(kalman *filter, double dot_angle, double dt)
{
- // Static so these are kept off the stack.
+ // so not on the stack.
static double gyro_rate_unbiased;
static double Pdot_00;
static double Pdot_01;
static double Pdot_10;
static double Pdot_11;
- // Unbias our gyro.
- gyro_rate_unbiased= dot_angle - filter->x_bias;
+ // dot_angle is the gyro value.
+ gyro_rate_unbiased = dot_angle - filter->x_bias;
// Store our unbiased gyro estimate.
filter->x_rate= gyro_rate_unbiased;
- // Update the angle estimate.
+ // x_angle = x_angle + (measured gyro - x_bias)*dt from X = AX(k-1) + Bu(k-1)
filter->x_angle= filter->x_angle + (dot_angle - filter->x_bias)*dt;
// Compute the derivative of the covariance matrix
@@ -92,13 +81,12 @@
* Function Name: kalman_update *
* Return Value: none *
* Parameters: struct filter, measured angle value *
-* Description: Called when a new accelerometer angle *
-* measurement is available. Updates the estimated *
-* angle that will be used. *
+* Description: Called with new accelerometer angle *
+* Updates estimated angle *
*********************************************************************/
void kalman_update(kalman *filter, double angle_measured)
{
- // Static so these are kept off the stack.
+ // Static to keep off the stack.
static double y;
static double S;
static double K_0;
@@ -110,16 +98,16 @@
y= angle_measured - filter->x_angle;
// Compute the error estimate.
- // S = C P C' + R
+ // S = H P H' + R
S = filter->R_angle + filter->P_00;
// Compute the kalman filter gains.
- // K = P C' inv(S)
+ // K = P H' inv(S) = P H'/(H P H' + R)
K_0 = filter->P_00 / S;
K_1 = filter->P_10 / S;
// Update covariance matrix.
- // P = P - K C P
+ // P = P - K H P = (I - KH)P
filter->P_00 -= K_0 * filter->P_00;
filter->P_01 -= K_0 * filter->P_01;
filter->P_10 -= K_1 * filter->P_00;
@@ -127,7 +115,7 @@
// Update the state (new)estimates (Correct the prediction of the state).
// Also adjust the bias on the gyro at every iteration.
- // x = x + K * y
+ // x = x + K * y = x + K(z - Hx )
filter->x_angle = filter->x_angle + K_0 * y;
filter->x_bias = filter->x_bias + K_1 * y;