求助：陀螺仪加速度计那个卡尔曼滤波例子中的几个疑惑之处？
//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;//过程协方差矩阵的微分矩阵
float angle_err;//角度偏量
float E;//计算的过程量
static float angle = 0; //下时刻最优估计值角度
static float q_bias = 0; //陀螺仪的偏差
static float P = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵
Rate = gyro_m - q_bias;
//计算过程协方差矩阵的微分矩阵
Pdot = Q_angle - P - P;//？？？？？？
Pdot = - P;
Pdot = - P;
Pdot = Q_gyro;//？？？？？？
angle += Rate * dt; //角速度积分得出角度
P += Pdot * dt; //计算协方差矩阵
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
angle_err = incAngle - angle; //计算角度偏差
E = R_angle + P;
K_0 = P / E; //计算卡尔曼增益
K_1 = P / E;
Y_0 = P;
Y_1 = P;
P -= K_0 * Y_0; //跟新协方差矩阵
P -= K_0 * Y_1;
P -= K_1 * Y_0;
P -= K_1 * Y_1;
angle += K_0 * angle_err; //给出最优估计值
q_bias += K_1 * angle_err;//跟新最优估计值偏差
return angle;
}
================================================================
问题1：
Pdot = Q_angle - P - P;//？？？？？？
Pdot = - P;
Pdot = - P;
Pdot = Q_gyro;];//？？？？？？
这几个式子是怎么来的？
Pdot = A*P + P*A' + Q----中的Q是什么矩阵？是{ {Q_angle, 0},{0,Q_gyro}}吗？转化后应该是：
Pdot = Q_angle; /* 0,0 */
Pdot = - 1; /* 0,1 */
Pdot = - 1; /* 1,0 */
Pdot = Q_gyro; /* 1,1 */
？？？
问题2：
P的更新好理解，P、P、P的更新莫名其妙的？
问题3：
陀螺仪的偏差为什么可以用：q_bias += K_1 * angle_err;表示？一般介绍卡尔曼滤波的资料上没有提到偏差更新 没有高手指导下吗？啊啊啊 同学 看样子你也为这个苦恼呢 网上基本上都是和你这个资料差不多 但貌似懂的人很少 我用这个程序调角度的时候漂移很严重 调了好久都不行 所以我也转过来看理论 希望自己编一个 但真的没那么容易。。。。。 还是先弄懂理论的吧。我决定先把相关的知识仔细研究下再看程序。。。 请问楼主，这种滤波效果怎样？谢谢 没人回答？我来抛个砖。。。。
问题1：A = [ 0 -1 ]
[ 00 ]
Pdot = A*P + P*A' + Q ，带进去就出来哒。。
Q是什么矩阵？是{ {Q_angle, 0},{0,Q_gyro}}——是的。。
问题2：看上面。
问题3：
q_bias 是什么，也是个状态量。。所以更新。。。
可看看http://www.ourdev.cn/forum.php?mod=viewthread&tid=4344801&highlight=%E9%99%80%E8%9E%BA%E4%BB%AA mark ，学习 連問題都描述不清楚，從何幫起？
mark,正在研究中 保留 研究下 自己按照第二个公式推倒一下吧，基本形式就是这个样子的，但是应为运算需要做了合理的化简，主要是舍去的dt的高阶小量，所以和原来的公式误差不大 顶一下，同时也学习一下。 看不懂，顶一下你 貌似看不懂啊。。。。 mqhrzy09 发表于 2012-4-24 14:28
没人回答？我来抛个砖。。。。
问题1：A = [ 0 -1 ]
[ 00 ]
你好，我最近在看卡尔曼滤波，在具体实现时看到了论坛里一个代码，http://www.amobbs.com/thread-5339449-1-1.html，有很多疑惑，不知可否与你详细交流一下？谢谢~ 我现在权限还不能加你为好友，可否QQ？ QQ：543257522
页:
[1]