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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 4088|回復(fù): 0
打印 上一主題 下一主題
收起左側(cè)

stm32f103rct6卡爾曼濾波源碼

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:323149 發(fā)表于 2018-5-5 23:00 | 只看該作者 回帖獎(jiǎng)勵(lì) |倒序?yàn)g覽 |閱讀模式
卡爾曼濾波源碼
以stm32f103rct6為例
測試串口通信數(shù)據(jù)
MPU數(shù)據(jù)進(jìn)行處理
檢測X軸加速度，Y軸角速度
參考網(wǎng)上例程

單片機(jī)源程序如下:
  1. #include "stm32f10x.h"
  2. #include <stdio.h>
  3. #include "delay.h"
  4. #include "sys.h"
  5. #include "Usart.h"
  6. #include "MPU6050.h"
  7. //******角度參數(shù)************
  8. float Gyro_y;        //Y軸陀螺儀數(shù)據(jù)暫存
  9. float Angle_gy;      //由角速度計(jì)算的傾斜角度
  10. float Accel_x;             //X軸加速度值暫存
  11. float Angle_ax;      //由加速度計(jì)算的傾斜角度
  12. float Angle;         //最終測量角度
  13. void TIM2_Configuration(void)
  14. {
  15.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;//定義一個(gè)定時(shí)器結(jié)構(gòu)體變量

  17.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定時(shí)器2
  18.   
  19.   TIM_TimeBaseStructure.TIM_Period = (10000 - 1);        //計(jì)數(shù)100次，因?yàn)閺?開始，所以減1
  20.   TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);        //時(shí)鐘72分頻，因?yàn)?不分頻，所以減1
  21.   TIM_TimeBaseStructure.TIM_ClockDivision = 0;        // 使用的采樣頻率之間的分頻比例
  22.    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;        //向上計(jì)數(shù)
  23.   TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);        //初始化定時(shí)器2

  25.   TIM_ClearITPendingBit(TIM2, TIM_IT_Update);                //清除定時(shí)器2中斷標(biāo)志位
  26.   TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);                //打開定時(shí)器2中斷

  28.   TIM_Cmd(TIM2, ENABLE);  //計(jì)數(shù)器使能，開始計(jì)數(shù)
  29. }

  31. void NVIC_Configuration(void)
  32. {
  33.   NVIC_InitTypeDef NVIC_InitStructure;
  34.   
  35.   NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0000);
  36.   NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

  38. /********************注冊定時(shí)器2中斷處理函數(shù)***********************/

  40.   NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
  41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  44.   NVIC_Init(&NVIC_InitStructure);

  46. /******************注冊串口1中斷服務(wù)函數(shù)************************/
  47.         NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//配置串口中斷
  48.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;//占先式優(yōu)先級設(shè)置為0
  49.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //副優(yōu)先級設(shè)置為0
  50.         NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//中斷禁止
  51.         NVIC_Init(&NVIC_InitStructure);//中斷初始化


  54. }

  56. /*************卡爾曼濾波*********************************/
  57. void Kalman_Filter(float Accel,float Gyro)               
  58. {
  59.                
  60.         static const float Q_angle=0.001;  
  61.         static const float Q_gyro=0.003;
  62.         static const float R_angle=0.5;
  63.         static const float dt=0.01;                          //dt為kalman濾波器采樣時(shí)間;
  64.         static const char  C_0 = 1;
  65.         static float Q_bias, Angle_err;
  66.         static float PCt_0, PCt_1, E;
  67.         static float K_0, K_1, t_0, t_1;
  68.         static float Pdot[4] ={0,0,0,0};
  69.         static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
  70.        
  71.         Angle+=(Gyro - Q_bias) * dt; //先驗(yàn)估計(jì)

  73.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分

  75.         Pdot[1]= -PP[1][1];
  76.         Pdot[2]= -PP[1][1];
  77.         Pdot[3]=Q_gyro;
  78.        
  79.         PP[0][0] += Pdot[0] * dt;   // Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
  80.         PP[0][1] += Pdot[1] * dt;   // =先驗(yàn)估計(jì)誤差協(xié)方差
  81.         PP[1][0] += Pdot[2] * dt;
  82.         PP[1][1] += Pdot[3] * dt;
  83.                
  84.         Angle_err = Accel - Angle;        //zk-先驗(yàn)估計(jì)
  85.        
  86.         PCt_0 = C_0 * PP[0][0];
  87.         PCt_1 = C_0 * PP[1][0];
  88.        
  89.         E = R_angle + C_0 * PCt_0;
  90.        
  91.         K_0 = PCt_0 / E;
  92.         K_1 = PCt_1 / E;
  93.        
  94.         t_0 = PCt_0;
  95.         t_1 = C_0 * PP[0][1];

  97.         PP[0][0] -= K_0 * t_0;                 //后驗(yàn)估計(jì)誤差協(xié)方差
  98.         PP[0][1] -= K_0 * t_1;
  99.         PP[1][0] -= K_1 * t_0;
  100.         PP[1][1] -= K_1 * t_1;
  101.                
  102.         Angle        += K_0 * Angle_err;         //后驗(yàn)估計(jì)
  103.         Q_bias        += K_1 * Angle_err;         //后驗(yàn)估計(jì)
  104.         Gyro_y   = Gyro - Q_bias;         //輸出值(后驗(yàn)估計(jì))的微分=角速度
  105. }
  106. void Angle_Calculate(void)
  107. {
  108.         static        uint8_t DataBuffer[2];        //數(shù)據(jù)緩存
  109. /****************************加速度****************************************/
  110.         I2C_ReadBuffer(DataBuffer, ACCEL_XOUT_H, 2);
  111.         Accel_x  = (short)((DataBuffer[0]<<8)+DataBuffer[1]);          //讀取X軸加速度
  112.         Angle_ax = (Accel_x +220) /16384;   //去除零點(diǎn)偏移,計(jì)算得到角度（弧度）
  113.         Angle_ax = Angle_ax*1.2*180/3.14;     //弧度轉(zhuǎn)換為度,

  115. /****************************角速度****************************************/
  116.         I2C_ReadBuffer(DataBuffer, GYRO_YOUT_H, 2);
  117.         Gyro_y = (short)((DataBuffer[0]<<8)+DataBuffer[1]);              //靜止時(shí)角速度Y軸輸出為-18左右
  118.         Gyro_y = (Gyro_y + 18)/16.4;         //去除零點(diǎn)偏移，計(jì)算角速度值
  119. //                Angle_gy = Angle_gy + Gyro_y*0.01;  //角速度積分得到傾斜角度.       

  121. /***************************卡爾曼濾波+角度融合*************************************/
  122.         Kalman_Filter(Angle_ax,Gyro_y);       //卡爾曼濾波計(jì)算傾角

  124. /*******************************互補(bǔ)濾波******************************************/

  126. /*補(bǔ)償原理是取當(dāng)前傾角和加速度獲
  127.         得傾角差值進(jìn)行放大，然后與陀螺
  128.         儀角速度疊加后再積分，從而使傾
  129.         角最跟蹤為加速度獲得的角度0.5
  130.         為放大倍數(shù)，可調(diào)節(jié)補(bǔ)償度;
  131.         0.01為系統(tǒng)周期10ms       
  132. */       
  133. //        Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
  134.                                                                                                                                          
  135. }
  136. int main(void)
  137. {
  138.         Stm32_Clock_Init(6);//系統(tǒng)時(shí)鐘設(shè)置為外部晶振，9倍頻
  139.         delay_init(72);//系統(tǒng)SysTick初始化
  140.         Usart_Configuration();
  141.         TIM2_Configuration();
  142.         NVIC_Configuration();
  143.         I2C_Congiguration();
  144.         MPU6050_Init();
  145.         while (1)
  146.         {
  147.                 printf("%f        %f        ",Angle,Gyro_y);
  148.                 USART1_Send_Enter();       
  149.                 delay_ms(100);
  150.         }
  151. }
  152. /****************定時(shí)器2中斷服務(wù)函數(shù)***************************************/
  153. void TIM2_IRQHandler(void)
  154. {
  155.         if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
  156.   {
  157.                 TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
  158.                 Angle_Calculate();
  159.         }
  160. }
復(fù)制代碼

所有資料51hei提供下載:
卡爾曼濾波.rar (427.33 KB, 下載次數(shù): 68)


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

使用道具 舉報(bào)

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

本版積分規(guī)則

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

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

快速回復(fù) 返回頂部 返回列表
主站蜘蛛池模板： 亚洲激情在线观看 | 欧美一区二区免费在线 | 久久久国产精品一区 | 欧美日日 | 成人亚洲精品久久久久软件 | 精品国产乱码久久久久久蜜柚 | 精品乱码一区二区三四区视频 | 久久精品91久久久久久再现 | 91在线播| 精品久久久久一区二区国产 | 不卡一区 | 男人av在线| 成人性视频免费网站 | 麻豆av一区二区三区久久 | 久久久.com | 久久精品91久久久久久再现 | 日韩精品在线观看免费 | 在线观看中文字幕dvd播放 | 91视频a | 欧美理论片在线观看 | 国产精品中文字幕在线 | 精品国产成人 | 成人h视频在线 | 久久久青草婷婷精品综合日韩 | 日本黄色片免费在线观看 | 日本久久久久久 | 国产99免费| 中文字幕亚洲精品在线观看 | 91在线精品视频 | 欧美精品在线免费观看 | 亚洲综合在线一区二区 | 国产精品久久久久久久免费观看 | 激情91| 国产资源在线视频 | 精品国产乱码久久久久久丨区2区 | 欧美日韩在线一区 | 亚洲高清视频一区二区 | 亚洲成人免费在线观看 | 欧美日韩一区不卡 | 久草网址 | 日韩一区二 |