新手请教一下卡尔曼滤波
[ 本帖最后由 oifgjhk 于 2014-3-5 23:09 编辑 ]\n\n[ 本帖最后由 oifgjhk 于 2014-3-5 23:08 编辑 ]\n\n请大神帮忙看看，我这样理解卡尔曼滤波对不对...新手，还请见谅#ifndef KALMAN_H
#define KALMAN_H
using namespace std;
class kalman;
class MPU_measure{
friend class kalman;
public:
MPU_measure(float f1, float f2) :angle_m(f1), gyro_m(f2){}
MPU_measure() = default;
private:
float angle_m;//加速度计测得的角度
float gyro_m;//陀螺仪测得的角度
};
class kalman:MPU_measure{
public:
kalman(float f1, float f2) :angle(0), angle_dot(0), q_bias(0), angle_err(0), Kg_0(0), Kg_1(0), MPU_measure(f1,f2){
P = 1;
P = 0;
P = 0;
P = 1;
P_process = 0;
P_process = 0;
P_process = 0;
P_process = 0;
}
float get_value(){
return angle;
}
//向前推算状态变量（先验估计）
void froward_forecast();
//向前推算误差协方差
void P_forecast();
//计算kalman增益
void calculate_Kg();
//后验状态估计
void backward_forecast();
//更新误差协方差
void P_update();
private:
//kalman 滤波后得到的角度
float angle, angle_dot;
//固定值：角度过程噪声， 陀螺仪过程噪声， 测量噪声， 时间微分
const float Q_angle = 0.001, Q_gyro = 0.003, R_angle = 0.5, dt = 0.0001;
//协方差计算过程中的Q 过程噪声协方差
float P_process;
//协方差: ( P:angle 协方差) (P)(P:bias 协方差)(P：陀螺仪 协方差)
float P;
//偏差 观测过程的革新
float q_bias, angle_err;
// angle kalman增益 ,bias kalman增益
float Kg_0, Kg_1;
const char A = 1, B = 1, H = 1;
};
//公式中^表示估计，-表示先验
void kalman::froward_forecast()
{
//公式 X^-(k)=AX^(k-1)+BU(k-1)+w(k)
angle = A*angle + (gyro_m - q_bias)*dt;//先验估计
}
void kalman:: P_forecast()
{
//公式P-(k)=AP(k-1)At+Q
P_process = Q_angle - P - P;
P_process = -P;
P_process = -P;
P_process = Q_gyro;
P = A*P*A + P_process * dt;
P = A*P*A + P_process * dt;
P = A*P*A + P_process * dt;
P = A*P*A + P_process * dt;
}
void kalman::calculate_Kg()
{
//公式K(k)=(P-(k)Ht)/(HP-(k)Ht+R)
Kg_0 = (P * H) / ((H*P * H) + R_angle);
Kg_1 = (P * H) / ((H*P * H) + R_angle);
}
void kalman::backward_forecast()
{
//公式X^(k)=X^-(k)+K(k)(Z(k)-HX^-(k))
angle = angle + Kg_0*(angle_m - H*angle);
q_bias = q_bias + Kg_1*(angle_m - H*angle);
}
void kalman:: P_update()
{
//公式P(k)=(1-K(k)H)P-(k)
P = P - Kg_0*H*P;
P = P - Kg_0*H*P;
P = P - Kg_1*H*P;
P = P - Kg_1*H*P;
}
#endif ....没人....这是要沉 :'(貌似总是近似贴合于陀螺仪.... 我现在已经对ＭＰＵ６０５０出来的原始数据进行了四元数转欧拉角的数据融合 融合的时候用的是互补滤波 看帖子好像要对原始数据进行卡尔曼滤波 楼主是这个顺序么
页:
[1]