新手请教一下卡尔曼滤波

oifgjhk

[ 本帖最后由 oifgjhk 于 2014-3-5 23:09 编辑 ]\n\n[ 本帖最后由 oifgjhk 于 2014-3-5 23:08 编辑 ]\n\n请大神帮忙看看，我这样理解卡尔曼滤波对不对...新手，还请见谅
[pre lang="c++" line="1"]#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[0][0] = 1;
                P[0][1] = 0;
                P[1][0] = 0;
                P[1][1] = 1;
                P_process[0] = 0;
                P_process[1] = 0;
                P_process[2] = 0;
                P_process[3] = 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[4];
        //协方差: ( P[0][0]:angle 协方差) (P[0][1])(P[1][0]:bias 协方差)(P[1][1]：陀螺仪 协方差)
        float P[2][2];
        //  偏差      观测过程的革新
        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[0] = Q_angle - P[0][1] - P[1][0];
        P_process[1] = -P[1][1];
        P_process[2] = -P[1][1];
        P_process[3] = Q_gyro;
        P[0][0] = A*P[0][0]*A + P_process[0] * dt;
        P[0][1] = A*P[0][1]*A + P_process[1] * dt;
        P[1][0] = A*P[1][0]*A + P_process[2] * dt;
        P[1][1] = A*P[1][1]*A + P_process[3] * dt;
}
void kalman::calculate_Kg()
{
        //公式K(k)=(P-(k)Ht)/(HP-(k)Ht+R)
        Kg_0 = (P[0][0] * H) / ((H*P[0][0] * H) + R_angle);
        Kg_1 = (P[1][0] * H) / ((H*P[0][0] * 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[0][0] = P[0][0] - Kg_0*H*P[0][0];
        P[0][1] = P[0][1] - Kg_0*H*P[0][1];
        P[1][0] = P[1][0] - Kg_1*H*P[0][0];
        P[1][1] = P[1][1] - Kg_1*H*P[0][1];
}

#endif[/code]

oifgjhk

....没人....这是要沉

oifgjhk

  貌似总是近似贴合于陀螺仪....

詹波波

我现在已经对ＭＰＵ６０５０出来的原始数据进行了四元数转欧拉角的数据融合　融合的时候用的是互补滤波　看帖子好像要对原始数据进行卡尔曼滤波　楼主是这个顺序么