zoukankan      html  css  js  c++  java
  • 卡尔曼滤波算法及其代码

     

    下面整篇文章都是转载的。

    最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Kолмогоров等人的研究工作,后人统称为维纳滤波理论。从理论上说,维纳滤波的最大缺点是必须用到无限过去的数据,不适用于实时处理。为了克服这一缺点,60年代Kalman把状态空间模型引入滤波理论,并导出了一套递推估计算法,后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则,来寻求一套递推估计的算法,其基本思想是:采用信号与噪声的状态空间模型,利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计,求出现时刻的估计值。它适合于实时处理和计算机运算。

    现设线性时变系统的离散状态防城和观测方程为:

    X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

    Y(k) = H(k)·X(k)+N(k)

    其中

    X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

    F(k,k-1)为状态转移矩阵

    U(k)为k时刻动态噪声

    T(k,k-1)为系统控制矩阵

    H(k)为k时刻观测矩阵

    N(k)为k时刻观测噪声

    则卡尔曼滤波的算法流程为:

    1. 预估计X(k)^= F(k,k-1)·X(k-1)
    2. 计算预估计协方差矩阵
      C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
      Q(k) = U(k)×U(k)'
    3. 计算卡尔曼增益矩阵
      K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
      R(k) = N(k)×N(k)'
    4. 更新估计
      X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]
    5. 计算更新后估计协防差矩阵
      C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'
    6. X(k+1) = X(k)~
      C(k+1) = C(k)~
      重复以上步骤

    其c语言实现代码如下:

    #include "stdlib.h"
    #include "rinv.c"
    int lman(n,m,k,f,q,r,h,y,x,p,g)
    int n,m,k;
    double f[],q[],r[],h[],y[],x[],p[],g[];
    { int i,j,kk,ii,l,jj,js;
    double *e,*a,*b;
    e=malloc(m*m*sizeof(double));
    l=m;
    if (l<n) l=n;
    a=malloc(l*l*sizeof(double));
    b=malloc(l*l*sizeof(double));
    for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
    { ii=i*l+j; a[ii]=0.0;
    for (kk=0; kk<=n-1; kk++)
    a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];
    }
    for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
    { ii=i*n+j; p[ii]=q[ii];
    for (kk=0; kk<=n-1; kk++)
    p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];
    }
    for (ii=2; ii<=k; ii++)
    { for (i=0; i<=n-1; i++)
    for (j=0; j<=m-1; j++)
    { jj=i*l+j; a[jj]=0.0;
    for (kk=0; kk<=n-1; kk++)
    a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];
    }
    for (i=0; i<=m-1; i++)
    for (j=0; j<=m-1; j++)
    { jj=i*m+j; e[jj]=r[jj];
    for (kk=0; kk<=n-1; kk++)
    e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];
    }
    js=rinv(e,m);
    if (js==0)
    { free(e); free(a); free(b); return(js);}
    for (i=0; i<=n-1; i++)
    for (j=0; j<=m-1; j++)
    { jj=i*m+j; g[jj]=0.0;
    for (kk=0; kk<=m-1; kk++)
    g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];
    }
    for (i=0; i<=n-1; i++)
    { jj=(ii-1)*n+i; x[jj]=0.0;
    for (j=0; j<=n-1; j++)
    x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j];
    }
    for (i=0; i<=m-1; i++)
    { jj=i*l; b[jj]=y[(ii-1)*m+i];
    for (j=0; j<=n-1; j++)
    b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];
    }
    for (i=0; i<=n-1; i++)
    { jj=(ii-1)*n+i;
    for (j=0; j<=m-1; j++)
    x[jj]=x[jj]+g[i*m+j]*b[j*l];
    }
    if (ii<k)
    { for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
    { jj=i*l+j; a[jj]=0.0;
    for (kk=0; kk<=m-1; kk++)
    a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j];
    if (i==j) a[jj]=1.0+a[jj];
    }
    for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
    { jj=i*l+j; b[jj]=0.0;
    for (kk=0; kk<=n-1; kk++)
    b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j];
    }
    for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
    { jj=i*l+j; a[jj]=0.0;
    for (kk=0; kk<=n-1; kk++)
    a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk];
    }
    for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
    { jj=i*n+j; p[jj]=q[jj];
    for (kk=0; kk<=n-1; kk++)
    p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk];
    }
    }
    }
    free(e); free(a); free(b);
    return(js);
    }

    C++实现代码如下:

    ============================kalman.h================================
    // kalman.h: interface for the kalman class.
    //
    //////////////////////////////////////////////////////////////////////
    #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)
    #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_
    #if _MSC_VER > 1000
    #pragma once
    #endif // _MSC_VER > 1000
    #include <math.h>
    #include "cv.h"
    class kalman
    {
    public:
    void init_kalman(int x,int xv,int y,int yv);
    CvKalman* cvkalman;
    CvMat* state;
    CvMat* process_noise;
    CvMat* measurement;
    const CvMat* prediction;
    CvPoint2D32f get_predict(float x, float y);
    kalman(int x=0,int xv=0,int y=0,int yv=0);
    //virtual ~kalman();
    };
    #endif // !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)
    ============================kalman.cpp================================
    #include "kalman.h"
    #include <stdio.h>
    /* tester de printer toutes les valeurs des vecteurs*/
    /* tester de changer les matrices du noises */
    /* replace state by cvkalman->state_post ??? */
    CvRandState rng;
    const double T = 0.1;
    kalman::kalman(int x,int xv,int y,int yv)
    {
    cvkalman = cvCreateKalman( 4, 4, 0 );
    state = cvCreateMat( 4, 1, CV_32FC1 );
    process_noise = cvCreateMat( 4, 1, CV_32FC1 );
    measurement = cvCreateMat( 4, 1, CV_32FC1 );
    int code = -1;
    /* create matrix data */
    const float A[] = {
    1, T, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, T,
    0, 0, 0, 1
    };
    const float H[] = {
    1, 0, 0, 0,
    0, 0, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 0
    };
    const float P[] = {
    pow(320,2), pow(320,2)/T, 0, 0,
    pow(320,2)/T, pow(320,2)/pow(T,2), 0, 0,
    0, 0, pow(240,2), pow(240,2)/T,
    0, 0, pow(240,2)/T, pow(240,2)/pow(T,2)
    };
    const float Q[] = {
    pow(T,3)/3, pow(T,2)/2, 0, 0,
    pow(T,2)/2, T, 0, 0,
    0, 0, pow(T,3)/3, pow(T,2)/2,
    0, 0, pow(T,2)/2, T
    };
    const float R[] = {
    1, 0, 0, 0,
    0, 0, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 0
    };
    cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
    cvZero( measurement );
    cvRandSetRange( &rng, 0, 0.1, 0 );
    rng.disttype = CV_RAND_NORMAL;
    cvRand( &rng, state );
    memcpy( cvkalman->transition_matrix->data.fl, A, sizeof(A));
    memcpy( cvkalman->measurement_matrix->data.fl, H, sizeof(H));
    memcpy( cvkalman->process_noise_cov->data.fl, Q, sizeof(Q));
    memcpy( cvkalman->error_cov_post->data.fl, P, sizeof(P));
    memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R));
    //cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) );
    //cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1));
    //cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );
    /* choose initial state */
    state->data.fl[0]=x;
    state->data.fl[1]=xv;
    state->data.fl[2]=y;
    state->data.fl[3]=yv;
    cvkalman->state_post->data.fl[0]=x;
    cvkalman->state_post->data.fl[1]=xv;
    cvkalman->state_post->data.fl[2]=y;
    cvkalman->state_post->data.fl[3]=yv;
    cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl[0]), 0 );
    cvRand( &rng, process_noise );
    }
    CvPoint2D32f kalman::get_predict(float x, float y){
    /* update state with current position */
    state->data.fl[0]=x;
    state->data.fl[2]=y;
    /* predict point position */
    /* x'k=A鈥k+B鈥k
    P'k=A鈥k-1*AT + Q */
    cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 );
    cvRand( &rng, measurement );
    /* xk=A?xk-1+B?uk+wk */
    cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post );
    /* zk=H?xk+vk */
    cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement );
    /* adjust Kalman filter state */
    /* Kk=P'k鈥T鈥?H鈥'k鈥T+R)-1
    xk=x'k+Kk鈥?zk-H鈥'k)
    Pk=(I-Kk鈥)鈥'k */
    cvKalmanCorrect( cvkalman, measurement );
    float measured_value_x = measurement->data.fl[0];
    float measured_value_y = measurement->data.fl[2];
    const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );
    float predict_value_x = prediction->data.fl[0];
    float predict_value_y = prediction->data.fl[2];
    return(cvPoint2D32f(predict_value_x,predict_value_y));
    }
    void kalman::init_kalman(int x,int xv,int y,int yv)
    {
    state->data.fl[0]=x;
    state->data.fl[1]=xv;
    state->data.fl[2]=y;
    state->data.fl[3]=yv;
    cvkalman->state_post->data.fl[0]=x;
    cvkalman->state_post->data.fl[1]=xv;
    cvkalman->state_post->data.fl[2]=y;
    cvkalman->state_post->data.fl[3]=yv;
    }

  • 相关阅读:
    SAP S/4HANA extensibility扩展原理介绍
    SAP CRM系统订单模型的设计与实现
    使用nodejs代码在SAP C4C里创建Individual customer
    SAP Cloud for Customer Account和individual customer的区别
    Let the Balloon Rise map一个数组
    How Many Tables 简单并查集
    Heap Operations 优先队列
    Arpa’s obvious problem and Mehrdad’s terrible solution 思维
    Passing the Message 单调栈两次
    The Suspects 并查集
  • 原文地址:https://www.cnblogs.com/xiaoming1989/p/2387798.html
Copyright © 2011-2022 走看看