久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步,快速開始

帖子
查看: 2955|回復: 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;
}               




分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏1 分享淘帖 頂 踩
回復

使用道具 舉報

沙發
ID:430308 發表于 2018-12-31 12:30 | 只看該作者
謝謝謝謝
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板: 精品久久久久久久久久久久久久久久 | 色香蕉网 | 性欧美8khd高清极品 | 一区二区三区高清 | 激情做爰呻吟视频舌吻 | 影音先锋在线观看视频 | www.亚洲国产 | 三级网站在线 | 看免费毛片 | 日本a网站| 一区二区三区视频在线 | 亚洲第一区在线观看 | 日本加勒比在线观看 | av福利网 | 国产欧美一区二区精品忘忧草 | 放几个免费的毛片出来看 | 久草视频免费在线 | 亚洲精品自拍视频 | 欧美 日韩 国产 成人 在线 | 日韩av一区二区在线观看 | 国产精品久久久久久久午夜 | a级片在线观看 | 亚洲免费久久 | 中文字幕在线观看一区二区三区 | 欧美精品一区在线观看 | 日韩精品视频在线 | 正在播放国产精品 | 三级网站在线 | 又黄又爽又色视频 | 亚洲一区在线视频 | 欧美三级大片 | 日日摸天天添天天添破 | 亚洲一区二区免费视频 | 欧美一区二区三区视频 | 日本高清视频www | 日本特黄特色aaa大片免费 | eeuss一区二区三区 | 欧美综合在线视频 | 九九九精品视频 | 国产一及片 | 在线观看视频一区二区三区 |