设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › 尔曼滤波器英文资料与C语言源程序
返回列表 发新帖
查看: 1052|回复: 0
打印 上一主题 下一主题
收起左侧

尔曼滤波器英文资料与C语言源程序

[复制链接]
跳转到指定楼层
楼主
ID:847251 发表于 2020-11-22 14:32 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
详细介绍卡尔曼滤波器。
附有程序源码


源程序如下:
  1. /***************************************************************************/
  2. /* kalman.c                                                                */
  3. /* 1-D Kalman filter Algorithm, using an inclinometer and gyro             */
  4. /* Author: Rich Chi Ooi                                                    */
  5. /* Version: 1.0                                                            */
  6. /* Date:30.05.2003                                                         */
  7. /* Adapted from Trammel Hudson(hudson@rotomotion.com)                      */  
  8. /* -------------------------------                                         */
  9. /* Compilation  procedure:                                                 */
  10. /*       Linux                                                             */
  11. /*      gcc68 -c  XXXXXX.c (to create object file)                         */
  12. /*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
  13. /*Upload data :                                                                   */
  14. /*        ul filename.txt                                                                 */
  15. /***************************************************************************/
  16. /* In this version:                                                        */
  17. /***************************************************************************/
  18. /* This is a free software; you can redistribute it and/or modify          */
  19. /* it under the terms of the GNU General Public License as published       */
  20. /* by the Free Software Foundation; either version 2 of the License,       */
  21. /* or (at your option) any later version.                                  */
  22. /*                                                                         */
  23. /* this code is distributed in the hope that it will be useful,            */
  24. /* but WITHOUT ANY WARRANTY; without even the implied warranty of          */
  25. /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           */
  26. /* GNU General Public License for more details.                            */
  27. /*                                                                         */
  28. /* You should have received a copy of the GNU General Public License       */
  29. /* along with Autopilot; if not, write to the Free Software                */
  30. /* Foundation, Inc., 59 Temple Place, Suite 330, Boston,                   */
  31. /* MA  02111-1307  USA                                                     */
  32. /***************************************************************************/

  34. #include <math.h>
  35. #include "eyebot.h"

  37. /*
  38. * The state is updated with gyro rate measurement every 20ms
  39. * change this value if you update at a different rate.
  40. */

  42. static const float dt = 0.02;

  44. /*
  45. * The covariance matrix.This is updated at every time step to
  46. * determine how well the sensors are tracking the actual state.
  47. */
  48. static float P[2][2] = {{ 1, 0 },
  49.                         { 0, 1 }};

  51. /*
  52. * Our two states, the angle and the gyro bias.As a byproduct of computing
  53. * the angle, we also have an unbiased angular rate available.These are
  54. * read-only to the user of the module.
  55. */
  56. float angle;
  57. float q_bias;
  58. float rate;


  61. /*
  62. * The R represents the measurement covariance noise.R=E[vvT]
  63. * In this case,it is a 1x1 matrix that says that we expect
  64. * 0.1 rad jitter from the inclinometer.
  65. * for a 1x1 matrix in this case v = 0.1
  66. */
  67. static const float R_angle = 0.001 ;


  70. /*
  71. * Q is a 2x2 matrix that represents the process covariance noise.
  72. * In this case, it indicates how much we trust the inclinometer
  73. * relative to the gyros.
  74. */
  75. static const float Q_angle = 0.001;
  76. static const float Q_gyro  = 0.0015;


  79. /*
  80. * state_update is called every dt with a biased gyro measurement
  81. * by the user of the module.  It updates the current angle and
  82. * rate estimate.
  83. *
  84. * The pitch gyro measurement should be scaled into real units, but
  85. * does not need any bias removal.  The filter will track the bias.
  86. *
  87. *          A = [ 0 -1 ]
  88. *              [ 0  0 ]
  89. */
  90. void stateUpdate(const float q_m){

  92.         float q;
  93.         float Pdot[4];

  95.         /* Unbias our gyro */
  96.         q = q_m - q_bias;

  98.         /*
  99.          * Compute the derivative of the covariance matrix
  100.          * (equation 22-1)
  101.          *        Pdot = A*P + P*A' + Q
  102.          *
  103.          */
  104.         Pdot[0] = Q_angle - P[0][1] - P[1][0];        /* 0,0 */
  105.         Pdot[1] = - P[1][1];                        /* 0,1 */
  106.         Pdot[2] = - P[1][1];                         /* 1,0 */
  107.         Pdot[3] = Q_gyro;                        /* 1,1 */

  109.         /* Store our unbias gyro estimate */
  110.         rate = q;

  112.         /*
  113.          * Update our angle estimate
  114.          * angle += angle_dot * dt
  115.          *       += (gyro - gyro_bias) * dt
  116.          *       += q * dt
  117.          */
  118.         angle += q * dt;

  120.         /* Update the covariance matrix */
  121.         P[0][0] += Pdot[0] * dt;
  122.         P[0][1] += Pdot[1] * dt;
  123.         P[1][0] += Pdot[2] * dt;
  124.         P[1][1] += Pdot[3] * dt;
  125. }


  128. /*
  129. * kalman_update is called by a user of the module when a new
  130. * inclinoometer measurement is available.
  131. *
  132. * This does not need to be called every time step, but can be if
  133. * the accelerometer data are available at the same rate as the
  134. * rate gyro measurement.
  135. *
  136. *         H  = [ 1 0 ]
  137. *
  138. * because the angle measurement directly corresponds to the angle
  139. * estimate and the angle measurement has no relation to the gyro
  140. * bias.
  141. */
  142. void kalmanUpdate(const float incAngle)
  143. {
  144.         /* Compute our measured angle and the error in our estimate */
  145.         float angle_m = incAngle;
  146.         float angle_err = angle_m - angle;

  148.         /*
  149.          * h_0 shows how the state measurement directly relates to
  150.          * the state estimate.
  151.           *   
  152.          *      H = [h_0 h_1]
  153.          *
  154.          * The h_1 shows that the state measurement does not relate
  155.          * to the gyro bias estimate.  We don't actually use this, so
  156.          * we comment it out.
  157.          */
  158.         float h_0 = 1;
  159.         /* const float                h_1 = 0; */

  161.         /*
  162.          * Precompute PH' as the term is used twice
  163.          * Note that H[0,1] = h_1 is zero, so that term is not not computed
  164.          */
  165.         const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/
  166.         const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/

  168.         /*
  169.          * Compute the error estimate:
  170.          * (equation 21-1)
  171.          *
  172.          *        E = H P H' + R
  173.          */
  174.         float E = R_angle +(h_0 * PHt_0);

  176.         /*
  177.          * Compute the Kalman filter gains:
  178.          * (equation 21-2)
  179.          *
  180.          *        K = P H' inv(E)
  181.                 */
  182.         float K_0 = PHt_0 / E;
  183.         float K_1 = PHt_1 / E;
  184.                
  185.         /*
  186.          * Update covariance matrix:
  187.          * (equation 21-3)
  188.          *
  189.          *        P = P - K H P
  190.          * Let
  191.          *      Y = H P
  192.          */
  193.         float Y_0 = PHt_0;  /*h_0 * P[0][0]*/
  194.         float Y_1 = h_0 * P[0][1];
  195.          
  196.         P[0][0] -= K_0 * Y_0;
  197.         P[0][1] -= K_0 * Y_1;
  198.         P[1][0] -= K_1 * Y_0;
  199.         P[1][1] -= K_1 * Y_1;
  200.         
  201.         /*
  202.          * Update our state estimate:
  203.          *
  204.          *        Xnew = X + K * error
  205.          *
  206.          * err is a measurement of the difference in the measured state
  207.          * and the estimate state.  In our case, it is just the difference
  208.          * between the inclinometer measured angle and the estimated angle.
  209.          */
  210.         angle        += K_0 * angle_err;
  211.         q_bias        += K_1 * angle_err;
  212. }
复制代码

所有资料51hei提供下载:
卡尔曼.zip (580.13 KB, 下载次数: 13)



评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表