守望者--AIR技术交流

 找回密码
 立即注册

QQ登录

只需一步,快速开始

扫一扫,访问微社区

搜索
热搜: ANE FlasCC 炼金术
查看: 435|回复: 0

[音频分析] Kalman滤波算法的C++实现

[复制链接]
  • TA的每日心情
    擦汗
    2018-4-10 15:18
  • 签到天数: 447 天

    [LV.9]以坛为家II

    1742

    主题

    2094

    帖子

    13万

    积分

    超级版主

    Rank: 18Rank: 18Rank: 18Rank: 18Rank: 18

    威望
    562
    贡献
    29
    金币
    51691
    钢镚
    1422

    开源英雄守望者

    发表于 2015-8-20 15:06:15 | 显示全部楼层 |阅读模式
    头文件:
    1. /*
    2. * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
    3. *
    4. * This program is free software; you can redistribute it and/or modify it
    5. * under the terms of the GNU General Public License as published by the
    6. * Free Software Foundation, either version 2 or any later version.
    7. *
    8. * Redistribution and use in source and binary forms, with or without
    9. * modification, are permitted provided that the following conditions are met:
    10. *
    11. * 1. Redistributions of source code must retain the above copyright notice,
    12. *    this list of conditions and the following disclaimer.
    13. *
    14. * 2. Redistributions in binary form must reproduce the above copyright
    15. *    notice, this list of conditions and the following disclaimer in the
    16. *    documentation and/or other materials provided with the distribution.
    17. *
    18. * This program is distributed in the hope that it will be useful, but WITHOUT
    19. * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
    20. * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
    21. * more details. A copy of the GNU General Public License is available at:
    22. * http://www.fsf.org/licensing/licenses
    23. */


    24. /*****************************************************************************
    25. *                                   kalman.h
    26. *
    27. * Kalman Filter.
    28. *
    29. * The Kalman filter is an efficient recursive filter that estimates the
    30. * internal state of a linear dynamic system from a series of noisy
    31. * measurements. In most applications, the internal state is much larger
    32. * (more degrees of freedom) than the few "observable" parameters which are
    33. * measured. However, by combining a series of measurements, the Kalman
    34. * filter can estimate the entire internal state.
    35. *
    36. * A wide variety of Kalman filters have now been developed, from Kalman's
    37. * original formulation, now called the simple Kalman filter, the Kalman-Bucy
    38. * filter, Schmidt's extended filter, the information filter, and a variety
    39. * of square-root filters that were developed by Bierman, Thornton and so on.
    40. *
    41. * Zhang Ming, 2010-10, Xi'an Jiaotong University.
    42. *****************************************************************************/


    43. #ifndef KALMAN_H
    44. #define KALMAN_H


    45. #include <vector.h>
    46. #include <matrix.h>
    47. #include <inverse.h>


    48. namespace splab
    49. {

    50.     template<typename Type>
    51.     Vector<Type> kalman( const Matrix<Type>&, const Matrix<Type>&,
    52.                          const Matrix<Type>&, const Matrix<Type>&,
    53.                          const Vector<Type>&, const Vector<Type>&,
    54.                          const Vector<Type>& );


    55.     #include <kalman-impl.h>

    56. }
    57. // namespace splab


    58. #endif
    59. // KALMAN_H
    复制代码


    实现文件:
    1. /*
    2. * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
    3. *
    4. * This program is free software; you can redistribute it and/or modify it
    5. * under the terms of the GNU General Public License as published by the
    6. * Free Software Foundation, either version 2 or any later version.
    7. *
    8. * Redistribution and use in source and binary forms, with or without
    9. * modification, are permitted provided that the following conditions are met:
    10. *
    11. * 1. Redistributions of source code must retain the above copyright notice,
    12. *    this list of conditions and the following disclaimer.
    13. *
    14. * 2. Redistributions in binary form must reproduce the above copyright
    15. *    notice, this list of conditions and the following disclaimer in the
    16. *    documentation and/or other materials provided with the distribution.
    17. *
    18. * This program is distributed in the hope that it will be useful, but WITHOUT
    19. * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
    20. * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
    21. * more details. A copy of the GNU General Public License is available at:
    22. * http://www.fsf.org/licensing/licenses
    23. */


    24. /*****************************************************************************
    25. *                              kalman-impl.h
    26. *
    27. * Implementation for Kalman Filter.
    28. *
    29. * Zhang Ming, 2010-10, Xi'an Jiaotong University.
    30. *****************************************************************************/


    31. /**
    32. * The simple Kalman filter for one step.
    33. * A ---> system matrix defining linear dynamic system
    34. * C ---> measurement matrix defining relationship between system's state
    35. *        and measurements
    36. * Q ---> covariance matrix of process noise in system state dynamics
    37. * R ---> covariance matrix of measurements uncertainty
    38. * y ---> measurements at time t
    39. * xPrev ---> previous estimated state vector of the linear dynamic system
    40. * initDiagV ---> diagonal vector for initializing the covariance matrix of
    41. *                state estimation uncertainty
    42. */
    43. template <typename Type>
    44. Vector<Type> kalman( const Matrix<Type> &A, const Matrix<Type> &C,
    45.                      const Matrix<Type> &Q, const Matrix<Type> &R,
    46.                      const Vector<Type> &xPrev, const Vector<Type> &y,
    47.                      const Vector<Type> &initDiagV )
    48. {
    49.     int N = xPrev.size();

    50.     // covariance matrix of state estimation uncertainty
    51.     static Matrix<Type> V = diag(initDiagV);

    52.     // previoused state vector
    53.     Vector<Type> xPred = A * xPrev;

    54.     // inovation
    55.     Vector<Type> alpha = y - C * xPred;

    56.     Matrix<Type> CTran = trT( C );
    57.     Matrix<Type> VPred = A*V*trT(A) + Q;

    58.     // Kalman gain matrix
    59.     Matrix<Type> KGain = VPred*CTran * inv(C*VPred*CTran+R);

    60.     V = ( eye(N,Type(1.0)) - KGain*C ) * VPred;

    61.     // return the estimation of the state vector
    62.     return xPred + KGain * alpha;
    63. }
    复制代码
    测试代码:
    1. /*****************************************************************************
    2. *                                   kalman.h
    3. *
    4. * Kalman filter testing.
    5. *
    6. * Zhang Ming, 2010-10, Xi'an Jiaotong University.
    7. *****************************************************************************/


    8. #define BOUNDS_CHECK

    9. #include <iostream>
    10. #include <kalman.h>


    11. using namespace std;
    12. using namespace splab;


    13. typedef double  Type;
    14. const   int     N = 2;
    15. const   int     M = 2;
    16. const   int     T = 20;


    17. int main()
    18. {
    19.     Matrix<Type> A(N,N), C(M,N), Q(N,N), R(M,M);
    20.     A = eye( N, Type(1.0) );    C = eye( N, Type(1.0) );
    21.     Q = eye( N, Type(1.0) );    R = eye( N, Type(2.0) );

    22.     Vector<Type> x(N,Type(1.0)), y(M), ytInit(M);
    23.     ytInit[0] = Type(0.5);  ytInit[1] = Type(2.0);
    24.     Matrix<Type> yt(M,T);
    25.     for( int t=0; t<T; ++t )
    26.         yt.setColumn( ytInit, t );

    27.     Vector<Type> intV( N, Type(10.0) );
    28.     for( int t=0; t<T; ++t )
    29.     {
    30.         y = yt.getColumn(t);
    31.         x = kalman( A, C, Q, R, x, y, intV );
    32.         cout << "Estimation of xt at the " << t << "th iteratin:   " << x << endl;
    33.     }

    34.     cout << "The theoretical xt should converge to:   " << ytInit << endl;

    35.     return 0;
    36. }
    复制代码
    运行结果:
    1. Estimation of xt at the 0th iteratin:   size: 2 by 1
    2. 0.576923
    3. 1.84615

    4. Estimation of xt at the 1th iteratin:   size: 2 by 1
    5. 0.532787
    6. 1.93443

    7. Estimation of xt at the 2th iteratin:   size: 2 by 1
    8. 0.51581
    9. 1.96838

    10. Estimation of xt at the 3th iteratin:   size: 2 by 1
    11. 0.507835
    12. 1.98433

    13. Estimation of xt at the 4th iteratin:   size: 2 by 1
    14. 0.503909
    15. 1.99218

    16. Estimation of xt at the 5th iteratin:   size: 2 by 1
    17. 0.501953
    18. 1.99609

    19. Estimation of xt at the 6th iteratin:   size: 2 by 1
    20. 0.500977
    21. 1.99805

    22. Estimation of xt at the 7th iteratin:   size: 2 by 1
    23. 0.500488
    24. 1.99902

    25. Estimation of xt at the 8th iteratin:   size: 2 by 1
    26. 0.500244
    27. 1.99951

    28. Estimation of xt at the 9th iteratin:   size: 2 by 1
    29. 0.500122
    30. 1.99976

    31. Estimation of xt at the 10th iteratin:   size: 2 by 1
    32. 0.500061
    33. 1.99988

    34. Estimation of xt at the 11th iteratin:   size: 2 by 1
    35. 0.500031
    36. 1.99994

    37. Estimation of xt at the 12th iteratin:   size: 2 by 1
    38. 0.500015
    39. 1.99997

    40. Estimation of xt at the 13th iteratin:   size: 2 by 1
    41. 0.500008
    42. 1.99998

    43. Estimation of xt at the 14th iteratin:   size: 2 by 1
    44. 0.500004
    45. 1.99999

    46. Estimation of xt at the 15th iteratin:   size: 2 by 1
    47. 0.500002
    48. 2

    49. Estimation of xt at the 16th iteratin:   size: 2 by 1
    50. 0.500001
    51. 2

    52. Estimation of xt at the 17th iteratin:   size: 2 by 1
    53. 0.5
    54. 2

    55. Estimation of xt at the 18th iteratin:   size: 2 by 1
    56. 0.5
    57. 2

    58. Estimation of xt at the 19th iteratin:   size: 2 by 1
    59. 0.5
    60. 2

    61. The theoretical xt should converge to:   size: 2 by 1
    62. 0.5
    63. 2


    64. Process returned 0 (0x0)   execution time : 0.125 s
    65. Press any key to continue.
    复制代码



    本文来自:http://my.oschina.net/zmjerry/blog/8517
    守望者AIR技术交流社区(www.airmyth.com)
    回复

    使用道具 举报

    您需要登录后才可以回帖 登录 | 立即注册

    本版积分规则

    
    关闭

    站长推荐上一条 /4 下一条

    QQ|手机版|Archiver|网站地图|小黑屋|守望者 ( 京ICP备14061876号

    GMT+8, 2019-8-19 01:01 , Processed in 0.038381 second(s), 33 queries .

    守望者AIR

    守望者AIR技术交流社区

    本站成立于 2014年12月31日

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