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

標題: 卡爾曼濾波程序 [打印本頁]

作者: liuqq 時間: 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;
}

作者: wubingbing 時間: 2018-12-31 12:30
謝謝謝謝

歡迎光臨 (http://m.zg4o1577.cn/bbs/)