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

 找回密碼
 立即注冊(cè)

QQ登錄

只需一步,快速開始

搜索
查看: 2905|回復(fù): 1
收起左側(cè)

卡爾曼濾波程序

[復(fù)制鏈接]
ID:80436 發(fā)表于 2015-5-21 22:45 | 顯示全部樓層 |閱讀模式
#define Q_angle             0.001       // 加速度計(jì)過程噪聲協(xié)方差(過程噪聲協(xié)方差矩陣對(duì)角元素之一)
#define Q_gyro              0.003       // 陀螺儀  過程噪聲協(xié)方差(過程噪聲協(xié)方差矩陣對(duì)角元素之一)
#define R_angle             0.5         // 測(cè)量噪聲協(xié)方差(不確定因素,故取估計(jì)值0.3~0.5)
#define dt                  0.01        // 卡爾曼采樣時(shí)間,同建模中的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濾波器采樣時(shí)間
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 根據(jù)上一狀態(tài)的最優(yōu)協(xié)方差P(k-1|k-1)推出當(dāng)前協(xié)方差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) 測(cè)量值 - 預(yù)測(cè)值

    PCt_0 = C_0 * PP[0][0];         //P(k|k-1) = P(k|k-1)*C_0  當(dāng)前協(xié)方差  C_0=協(xié)方差變換增益
    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測(cè)量噪聲

    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;
}               




回復(fù)

使用道具 舉報(bào)

ID:430308 發(fā)表于 2018-12-31 12:30 | 顯示全部樓層
謝謝謝謝
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

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

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表
主站蜘蛛池模板: 亚洲精品成人网 | www.久草| 美女视频三区 | 超碰最新在线 | 欧美精品导航 | 在线看片国产精品 | 国产精品久久久爽爽爽麻豆色哟哟 | 99免费在线| 高清免费在线 | 亚洲九九 | 免费的色网站 | 久久高清 | 国产精品精品视频一区二区三区 | 免费久| 久久专区| 免费在线一区二区三区 | 亚洲国产成人久久综合一区,久久久国产99 | 欧美最猛黑人xxxⅹ 粉嫩一区二区三区四区公司1 | 99视频在线免费观看 | 999精品视频| 亚洲精品久久久久国产 | 中文字幕免费中文 | 欧美激情精品久久久久 | 手机在线不卡av | 国产激情精品视频 | 成人免费观看视频 | 国产精品视频免费观看 | 欧美伊人 | 成人免费视频一区 | 亚洲精品专区 | 国产精品久久久久久久一区探花 | 黄色大片视频 | 国产小视频在线 | 日本色婷婷 | 国产不卡在线 | 亚洲一区二区三区免费在线 | 久久久久久国产精品三区 | 精品色 | 宅男噜噜噜66一区二区 | 久久久国产精品入口麻豆 | 99精品视频在线 |