卡尔曼滤波问题

风暴

我是第八届光电组新手，正在做直立部分调试，下面的卡尔曼滤波程序有些不懂，求解释！！！ 程序中的这几句有何作用呀？if(Gyro > 4090) Gyro += 1000; else if(Gyro > 4084) Gyro += 500; if(Gyro < 55) Gyro -= 1000; else if(Gyro < 60) Gyro -= 500; 那位大神知道呀？ 全部程序如下： #include "main.h" volatile float QingJiao = 0; volatile float Gyro_Data = 0; void kalman_update(void) { float Q =1,R = 300; static float RealData = 0,RealData_P = 0; float NowData = 0,NowData_P = 0; float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0; float Acc_x = 0,Acc_z = 0, Gyro = 0; static float his_acc = 0.0,his_accx = 0.0,his_accz = 0.0; while(!ATD0STAT0_SCF); //等待转换完成 Acc_x = (float)ATD0DR1; Acc_z = (float)ATD0DR2; Gyro = (float)ATD0DR0; if(Gyro > 4090) Gyro += 1000; else if(Gyro > 4084) Gyro += 500; if(Gyro < 55) Gyro -= 1000; else if(Gyro < 60) Gyro -= 500; Acc_x = Acc_x - 2342.0; Acc_z = Acc_z - 2076.0; Gyro = Gyro - 2048.0; Gyro_Data = Gyro; OutData[0] = Gyro_Data; accelerometer_angle = atan2f(-Acc_x,Acc_z); OutData[1] = accelerometer_angle*1000; gyroscope_rate = Gyro*0.0023; //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec. //(3300/4096)/(0.67*9.1)*(3.14/180) = 0.0023 NowData = RealData + gyroscope_rate*0.01; //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k) NowData_P = sqrt(Q*Q+RealData_P*RealData_P); //2.计算预估计协方差矩阵 P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k) Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R)); //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k)) RealData = NowData + Kg*(accelerometer_angle - NowData); //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1)) RealData_P = sqrt((1-Kg)*NowData_P*NowData_P); //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1) QingJiao = RealData; OutData[2] = QingJiao*1000; OutPut_Data(); }

十七岁不哭

gyroscope_rate = Gyro*0.0023; //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.

//(3300/4096)/(0.67*9.1)*(3.14/180) = 0.0023

楼主，你的放大倍数9.1是怎么测得？

xiongcaifei

楼主，我看到网上很流行一种卡尔曼滤波，但程序与你这个不一样，不知道有什么差别吗？？求楼主解答