求助关于卡尔曼滤波

 

// float gyro_m：陀螺仪测得的量（角速度）

 //float incAngle:加计测得的角度值

 

#define dt                  0.0015//卡尔曼滤波采样频率 

#define R_angle          0.69 //测量噪声的协方差（即是测量偏差）

#define Q_angle          0.0001//过程噪声的协方差 

#define Q_gyro           0.0003 //过程噪声的协方差  过程噪声协方差为一个一行两列矩阵



float kalmanUpdate(const float gyro_m,const float incAngle  

{         

        float K_0;//含有卡尔曼增益的另外一个函数，用于计算最优估计值

        float K_1;//含有卡尔曼增益的函数，用于计算最优估计值的偏差

        float Y_0;

        float Y_1;

        

        float Rate;//去除偏差后的角速度

        float Pdot[4];//过程协方差矩阵的微分矩阵

        float angle_err;//角度偏量

        float E;//计算的过程量

                

        static float angle = 0;            //下时刻最优估计值角度

        static float q_bias = 0;        //陀螺仪的偏差                 

        static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵

                  

        Rate = gyro_m - q_bias;

  

        //计算过程协方差矩阵的微分矩阵     

        Pdot[0] = Q_angle - P[0][1] - P[1][0];         

        Pdot[1] = - P[1][1];                         

        Pdot[2] = - P[1][1];                                  

        Pdot[3] = Q_gyro;                         



        angle += Rate * dt; //角速度积分得出角度



        P[0][0] += Pdot[0] * dt; //计算协方差矩阵

        P[0][1] += Pdot[1] * dt; 

        P[1][0] += Pdot[2] * dt; 

        P[1][1] += Pdot[3] * dt; 

  

        angle_err = incAngle - angle; //计算角度偏差



        E = R_angle + P[0][0];

        K_0 = P[0][0] / E; //计算卡尔曼增益

        K_1 = P[1][0] / E; 



        Y_0 = P[0][0];   

        Y_1 = 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; 





        angle += K_0 * angle_err; //给出最优估计值

        q_bias += K_1 * angle_err;//跟新最优估计值偏差 



        return angle; 

#include "ars.h"







#include <math.h>





/*

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





/*

* R represents the measurement covariance noise. In this case,

* it is a 1x1 matrix that says that we expect 0.3 rad jitter

* from the accelerometer.

*/

static const float R_angle = 0.3;





/*

* Q is a 2x2 matrix that represents the process covariance noise.

* In this case, it indicates how much we trust the acceleromter

* relative to the gyros.

*/

static const float Q_angle = 0.001;

static const float Q_gyro = 0.003;





/*

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

*

* Our state vector is:

*

* X = [ angle, gyro_bias ]

*

* It runs the state estimation forward via the state functions:

*

* Xdot = [ angle_dot, gyro_bias_dot ]

*

* angle_dot = gyro - gyro_bias

* gyro_bias_dot = 0

*

* And updates the covariance matrix via the function:

*

* Pdot = A*P + P*A' + Q

*

* A is the Jacobian of Xdot with respect to the states:

*

* A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]

* [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]

*

* = [ 0 -1 ]

* [ 0 0 ]

*

* Due to the small CPU available on the microcontroller, we've

* hand optimized the C code to only compute the terms that are

* explicitly non-zero, as well as expanded out the matrix math

* to be done in as few steps as possible. This does make it harder

* to read, debug and extend, but also allows us to do this with

* very little CPU time.

*/

void ars_predict(float gyro, float dt)

{

/* Unbias our gyro */

const float q = gyro - q_bias;



/*

* Compute the derivative of the covariance matrix

*

* Pdot = A*P + P*A' + Q

*

* We've hand computed the expansion of A = [ 0 -1, 0 0 ] multiplied

* by P and P multiplied by A' = [ 0 0, -1, 0 ]. This is then added

* to the diagonal elements of Q, which are Q_angle and Q_gyro.

*/

const float Pdot[2 * 2] = {

Q_angle - P[0][1] - P[1][0], /* 0,0 */

- P[1][1], /* 0,1 */

- P[1][1], /* 1,0 */

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

* accelerometer measurement is available. ax_m and az_m do not

* need to be scaled into actual units, but must be zeroed and have

* the same scale.

*

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

*

* For a two-axis accelerometer mounted perpendicular to the rotation

* axis, we can compute the angle for the full 360 degree rotation

* with no linearization errors by using the arctangent of the two

* readings.

*

* As commented in state_update, the math here is simplified to

* make it possible to execute on a small microcontroller with no

* floating point unit. It will be hard to read the actual code and

* see what is happening, which is why there is this extensive

* comment block.

*

* The C matrix is a 1x2 (measurements x states) matrix that

* is the Jacobian matrix of the measurement value with respect

* to the states. In this case, C is:

*

* C = [ d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ]

* = [ 1 0 ]

*

* because the angle measurement directly corresponds to the angle

* estimate and the angle measurement has no relation to the gyro

* bias.

*/

float

ars_update(const float angle_m)

{

/* Compute our measured angle and the error in our estimate */

const float angle_err = angle_m - angle;



/*

* C_0 shows how the state measurement directly relates to

* the state estimate.

*

* The C_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.

*/

const float C_0 = 1;

/* const float C_1 = 0; */



/*

* PCt<2,1> = P<2,2> * C'<2,1>, which we use twice. This makes

* it worthwhile to precompute and store the two values.

* Note that C[0,1] = C_1 is zero, so we do not compute that

* term.

*/

const float PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */

const float PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */



/*

* Compute the error estimate. From the Kalman filter paper:

* 

* E = C P C' + R

* 

* Dimensionally,

*

* E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>

*

* Again, note that C_1 is zero, so we do not compute the term.

*/

const float E =

R_angle

+ C_0 * PCt_0

/* + C_1 * PCt_1 = 0 */

;



/*

* Compute the Kalman filter gains. From the Kalman paper:

*

* K = P C' inv(E)

*

* Dimensionally:

*

* K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>

*

* Luckilly, E is <1,1>, so the inverse of E is just 1/E.

*/

const float K_0 = PCt_0 / E;

const float K_1 = PCt_1 / E;



/*

* Update covariance matrix. Again, from the Kalman filter paper:

*

* P = P - K C P

*

* Dimensionally:

*

* P<2,2> -= K<2,1> C<1,2> P<2,2>

*

* We first compute t<1,2> = C P. Note that:

*

* t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0]

*

* But, since C_1 is zero, we have:

*

* t[0,0] = C[0,0] * P[0,0] = PCt[0,0]

*

* This saves us a floating point multiply.

*/

const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */

const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */



P[0][0] -= K_0 * t_0;

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;



/*

* Update our state estimate. Again, from the Kalman paper:

*

* X += K * err

*

* And, dimensionally,

*

* X<2> = X<2> + K<2,1> * err<1,1>

*

* 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 two accelerometer measured angle and our estimated

* angle.

*/

angle += K_0 * angle_err;

q_bias += K_1 * angle_err;



return angle;

}