|
|
//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;
}
================================================================
问题1：
Pdot[0] = Q_angle - P[0][1] - P[1][0];//？？？？？？
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;];//？？？？？？
这几个式子是怎么来的？
Pdot = A*P + P*A' + Q----中的Q是什么矩阵？是{ {Q_angle, 0},{0,Q_gyro}}吗？转化后应该是：
Pdot[0] = Q_angle; /* 0,0 */
Pdot[1] = - 1; /* 0,1 */
Pdot[2] = - 1; /* 1,0 */
Pdot[3] = Q_gyro; /* 1,1 */
？？？
问题2：
P[0][0]的更新好理解，P[0][1]、P[1][0]、P[1][1]的更新莫名其妙的？
问题3：
陀螺仪的偏差为什么可以用：q_bias += K_1 * angle_err;表示？一般介绍卡尔曼滤波的资料上没有提到偏差更新 |
阿莫论坛20周年了！感谢大家的支持与爱护！！
曾经有一段真挚的爱情摆在我的面前，我没有珍惜，现在想起来，还好我没有珍惜……
|
发表于 2012-1-8 13:34:01
楼主
