找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 2566|回复: 1
收起左侧

卡尔曼滤波程序

[复制链接]
ID:80436 发表于 2015-5-21 22:45 | 显示全部楼层 |阅读模式
#define Q_angle             0.001       // 加速度计过程噪声协方差(过程噪声协方差矩阵对角元素之一)
#define Q_gyro              0.003       // 陀螺仪  过程噪声协方差(过程噪声协方差矩阵对角元素之一)
#define R_angle             0.5         // 测量噪声协方差(不确定因素,故取估计值0.3~0.5)
#define dt                  0.01        // 卡尔曼采样时间,同建模中的dt
float KF_Angle,KF_Angle_dot;
float angle, angle_dot;
//const float xdata Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;  //dt的取值为kalman滤波器采样时间
float PP[2][2] = {
                    { 1, 0 },
                    { 0, 1 }
                 };
float Pdot[4] ={0,0,0,0};    //////////////////
const char C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

void Kalman_Filter(float angle_temp,float angle_dot_temp)
{
    angle+=(angle_dot_temp-q_bias) * dt; //X(k|k-1)=A X(k-1|k-1)+B U(k) ……… (1)

    Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
    Pdot[1] =- PP[1][1];
    Pdot[2] =- PP[1][1];
    Pdot[3] = Q_gyro;
    //P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)    //Q过程噪声                                                            
    PP[0][0] += Pdot[0] * dt;       // P(k|k-1)=P(k-1|k-1)+Pdot[0]*dt 根据上一状态的最优协方差P(k-1|k-1)推出当前协方差P(k|k-1)
    PP[0][1] += Pdot[1] * dt;
    PP[1][0] += Pdot[2] * dt;       // PP[1][0]=PP[1][0]-PP[1][1]*dt;
    PP[1][1] += Pdot[3] * dt;       // PP = PP+Pdot*dt

    angle_err = angle_temp - angle; //Z(k)-X(k|k-1) 测量值 - 预测值

    PCt_0 = C_0 * PP[0][0];         //P(k|k-1) = P(k|k-1)*C_0  当前协方差  C_0=协方差变换增益
    PCt_1 = C_0 * PP[1][0];         //P(k|k-1) = P(k|k-1)*C_0

    E = R_angle + C_0 * PCt_0;  //H P(k|k-1) H’ + R     C_0=H*H'=1  R测量噪声

    K_0 = PCt_0 / E;        //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)   H=1
    K_1 = PCt_1 / E;        //

    t_0 = PCt_0;                //
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0;      //P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)    H=1
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;      // PP = PP-K*t


    angle   += K_0 * angle_err; //X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) H=1
    q_bias  += K_1 * angle_err; //X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) H=1
    angle_dot = angle_dot_temp-q_bias;
}               




回复

使用道具 举报

ID:430308 发表于 2018-12-31 12:30 | 显示全部楼层
谢谢谢谢
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表