之前上传的卡尔曼滤波代码的真正出处

zelight

/***************************************************************************/
/* kalman.c                                                                */
/* 1-D Kalman filter Algorithm, using an inclinometer and gyro             */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:30.05.2003                                                         */
/* Adapted from Trammel Hudson(hudson@rotomotion.com)                      */  
/* -------------------------------                                         */
/* Compilation  procedure:                                                 */
/*       Linux                                                             */
/*      gcc68 -c  XXXXXX.c (to create object file)                         */
/*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
/*Upload data :           */
/* ul filename.txt                       */
/***************************************************************************/
/* In this version:                                                        */
/***************************************************************************/
/* This is a free software; you can redistribute it and/or modify          */
/* it under the terms of the GNU General Public License as published       */
/* by the Free Software Foundation; either version 2 of the License,       */
/* or (at your option) any later version.                                  */
/*                                                                         */
/* this code is distributed in the hope that it will be useful,            */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of          */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           */
/* GNU General Public License for more details.                            */
/*                                                                         */
/* You should have received a copy of the GNU General Public License       */
/* along with Autopilot; if not, write to the Free Software                */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston,                   */
/* MA  02111-1307  USA                                                     */
/***************************************************************************/
#include <math.h>
#include "eyebot.h"
/*
* The state is updated with gyro rate measurement every 20ms
* change this value if you update at a different rate.
*/
static const float dt = 0.02;
/*
* The covariance matrix.This is updated at every time step to
* determine how well the sensors are tracking the actual state.
*/
static float P[2][2] = {{ 1, 0 },
                 { 0, 1 }};
/*
* Our two states, the angle and the gyro bias.As a byproduct of computing
* the angle, we also have an unbiased angular rate available.These are
* read-only to the user of the module.
*/
float angle;
float q_bias;
float rate;

/*
* The R represents the measurement covariance noise.R=E[vvT]
* In this case,it is a 1x1 matrix that says that we expect
* 0.1 rad jitter from the inclinometer.
* for a 1x1 matrix in this case v = 0.1
*/
static const float R_angle = 0.001 ;

/*
* Q is a 2x2 matrix that represents the process covariance noise.
* In this case, it indicates how much we trust the inclinometer
* relative to the gyros.
*/
static const float Q_angle = 0.001;
static const float Q_gyro  = 0.0015;

/*
* state_update is called every dt with a biased gyro measurement
* by the user of the module.  It updates the current angle and
* rate estimate.
*
* The pitch gyro measurement should be scaled into real units, but
* does not need any bias removal.  The filter will track the bias.
*
*   A = [ 0 -1 ]
*       [ 0  0 ]
*/
void stateUpdate(const float q_m){
float q;
float Pdot[4];
/* Unbias our gyro */
q = q_m - q_bias;//当前角速度：测量值-估计值
/*
  * Compute the derivative of the covariance matrix
  * (equation 22-1)
  * Pdot = A*P + P*A' + Q
  *
  */
Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
Pdot[1] = - P[1][1];          /* 0,1 */
Pdot[2] = - P[1][1];           /* 1,0 */
Pdot[3] = Q_gyro;   /* 1,1 */
/* Store our unbias gyro estimate */
rate = q;
/*
  * Update our angle estimate
  * angle += angle_dot * dt
  *       += (gyro - gyro_bias) * dt
  *       += q * dt
  */
angle += q * dt;//角速度积分累加到 估计角度
/* Update the covariance matrix */
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
}

/*
* kalman_update is called by a user of the module when a new
* inclinoometer measurement is available.
*
* This does not need to be called every time step, but can be if
* the accelerometer data are available at the same rate as the
* rate gyro measurement.
*
*         H  = [ 1 0 ]
*
* because the angle measurement directly corresponds to the angle
* estimate and the angle measurement has no relation to the gyro
* bias.
*/
void kalmanUpdate(const float incAngle)
{
/* Compute our measured angle and the error in our estimate */
float angle_m = incAngle;
float angle_err = angle_m - angle;//1.12 zk-H*xk_dot
/*
  * h_0 shows how the state measurement directly relates to
  * the state estimate.
   *   
  *      H = [h_0 h_1]
  *
  * The h_1 shows that the state measurement does not relate
  * to the gyro bias estimate.  We don't actually use this, so
  * we comment it out.
  */
float h_0 = 1;
/* const float  h_1 = 0; */
/*
  * Precompute PH' as the term is used twice
  * Note that H[0,1] = h_1 is zero, so that term is not not computed
  */
const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/
const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/
/*
  * Compute the error estimate:
  * (equation 21-1)
  *
  * E = H P H' + R
  */
float E = R_angle +(h_0 * PHt_0);
/*
  * Compute the Kalman filter gains:
  * (equation 21-2)
  *
  * K = P H' inv(E)
         */
float K_0 = PHt_0 / E;
float K_1 = PHt_1 / E;
  
/*
  * Update covariance matrix:
  * (equation 21-3)
  *
  * P = P - K H P
         * Let
  *      Y = H P
  */
float Y_0 = PHt_0;  /*h_0 * P[0][0]*/
float Y_1 = h_0 * P[0][1];
  
P[0][0] -= K_0 * Y_0;
P[0][1] -= K_0 * Y_1;
P[1][0] -= K_1 * Y_0;
P[1][1] -= K_1 * Y_1;

/*
  * Update our state estimate:
  *
  * Xnew = X + K * error
  *
  * err is a measurement of the difference in the measured state
  * and the estimate state.  In our case, it is just the difference
  * between the inclinometer measured angle and the estimated angle.
  */
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
}

whut_RY

谢谢！

jyh728

好贴

rguc

赞一个 这个代码的注释太全了

断翅at雄鹰

呵呵

二硫碘化钾

/*
   * Compute the derivative of the covariance matrix
   * (equation 22-1)
   * Pdot = A*P + P*A' + Q
  *
   */
请问这一句中： Pdot = A*P + P*A' + Q 是不是有问题？
kalman中应该是Pdot=A*P*A'+Q
我用matlab仿真了，如果Pdot = A*P + P*A' + Q的话，bias波动很严重，不收敛
但Pdot=A*P*A'+Q的话，效果就好多了
请问大家用这个程序仿真有问题没？如果程序是对的，那为什么Pdot = A*P + P*A' + Q？？？请高手解释一下

wo520

论坛元老还真是好学啊！！厉害！！

zhiweihappy

6楼说的，就是我要说的，请大侠们指点

csycaiyan

q_bias是什么呀，bias是什么呀。

hai_chuan

学习中