卡尔曼滤波器(一)
程序员文章站
2022-07-12 09:49:23
...
设每一次采样的观测值为Px,Py,Pz 所有采样值均默认服从正态分布和马尔可夫性(可能性均可按照发生概率运算)
假设采样频率是10次/秒
根据卡尔曼滤波算法
卡尔曼滤波器代码如下
float gyrox = -(gx - gxo) / GyroRatio * dt; //x轴角速度
float gyroy = -(gy - gyo) / GyroRatio * dt; //y轴角速度
float gyroz = -(gz - gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //x轴角速度积分
agz += gyroz;
/* kalman start */
Sx = 0;
Rx = 0;
Sy = 0;
Ry = 0;
Sz = 0;
Rz = 0;
for (int i = 1; i < 10; i++) {
//测量值平均值运算 ->加速度平均值
a_x[i - 1] = a_x[i];
Sx += a_x[i];
a_y[i - 1] = a_y[i];
Sy += a_y[i];
a_z[i - 1] = a_z[i];
Sz += a_z[i];
}
a_x[9] = aax;
Sx += aax;
Sx /= 10; //x轴加速度平均值
a_y[9] = aay;
Sy += aay;
Sy /= 10; //y轴加速度平均值
a_z[9] = aaz;
Sz += aaz;
Sz /= 10; //z轴加速度平均值
//计算方差
for (int i = 0; i < 10; i++) {
Rx += sq(a_x[i] - Sx);
Ry += sq(a_y[i] - Sy);
Rz += sq(a_z[i] - Sz);
}
Rx = Rx / 9; //方差
Ry = Ry / 9;
Rz = Rz / 9;
Px = Px + 0.0025; // 0.0025在下面有说明...
Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
Px = (1 - Kx) * Px; //更新p值
Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;
Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;
直白点说就是,每次更新一次观测周期内的观测值,用上一次的观测参与下一次的误差概率计算,然后得出下一次的去噪声值,以此类推。
转载于:https://my.oschina.net/VenusV/blog/1546153
上一篇: 滤波器之SG滤波器:(python实现)
下一篇: ros学习之topic实例3