极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 9424|回复: 0

kalman滤波器的传递函数怎么求?

[复制链接]
发表于 2013-10-22 21:55:03 | 显示全部楼层 |阅读模式
如下面程序所示的一个kalman滤波器
void Kalman_Filter(float gyro_m,float acc_m)  
//double P[2][2] = {{ 1, 0 },{ 0, 1 }};   double Pdot[4] ={ 0,0,0,0};   static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.007,C_0 = 1;
//double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;   double angle, angle_dot;   // aaxdot,aax;  double position_dot,position_dot_filter,positiono;
      {
            omega +=(acc_m-q_bias1) * dtt;   
            Pdot[0]=Q_angle - P[0][1] - P[1][0];  //
            Pdot[1]=- P[1][1];
            Pdot[2]=- P[1][1];
            Pdot[3]=Q_gyro;
            P[0][0] += Pdot[0] * dtt;
            P[0][1] += Pdot[1] * dtt;
            P[1][0] += Pdot[2] * dtt;
            P[1][1] += Pdot[3] * dtt;
            omega_err = gyro_m - omega;
            PCt_0 = C_0 * P[0][0];
            PCt_1 = C_0 * P[1][0];
            E = R_angle + C_0 * PCt_0;
            K_0 = PCt_0 / E;
            K_1 = PCt_1 / E;
            t_0 = PCt_0;
            t_1 = C_0 * P[0][1];
            P[0][0] -= K_0 * t_0;
            P[0][1] -= K_0 * t_1;
            P[1][0] -= K_1 * t_0;
            P[1][1] -= K_1 * t_1;
            omega +=  K_0 * omega_err;
            q_bias1 += K_1 * omega_err;   
      }

它的传递函数怎么求?!伯德图怎么画?(最好是用matlab
回复

使用道具 举报

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

本版积分规则

Archiver|联系我们|极客工坊

GMT+8, 2026-6-8 18:09 , Processed in 0.034090 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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