UKF的C、C++代码实现

老外写的一些UKF相关的C＋＋代码 对需要UKF编程的人有帮助, 学习卡尔曼滤波技术的最佳材料，易懂易看利于快速进行二次开发

/*
 * Kalman filter for Flying Fox
 *
 * from paparazzi autopilot
 * also by Zik Saleeba 2008-04-05
 */

#include 
#include 

#include “FreeRTOS.h“
#include “kalman.h“
#include “matrix.h“


/*
 * ukf_filter_new
 */

ukf_filter
ukf_filter_new（unsigned int state_dim
                           unsigned int measure_dim
                           double *Q
                           double *R
                           filter_function ffun
                           measure_function mfun） {
    ukf_filter filter;
    int Size;
    unsigned err = 0;
    // nothing to do if no state or measurement !
    if（state_dim == 0 || measure_dim == 0）
        return 0;
        // alloc new structure
        filter = pvPortMalloc（sizeof（struct ukf_filter_t））;
        // returns 0 if allocation fails
        if（filter == 0）
                return 0;
        // fills the structure
        filter->state_dim = state_dim;
        filter->measure_dim = measure_dim;
        filter->ffun = ffun;
        filter->mfun = mfun;

        filter->x = pvPortMalloc（state_dim * sizeof（double））;
        err |= （filter->x == 0）;

        filter->y = pvPortMalloc（measure_dim * sizeof（double））;
        err |= （filter->y == 0）;

        Size = state_dim * state_dim;

        filter->P = pvPortMalloc（Size * sizeof（double））;
        err |= （filter->P == 0）;

        Size = 2 * state_dim + 1;

        filter->wm = pvPortMalloc（Size * sizeof（double））;
        err |= （filter->wm == 0）;

        filter->wc = pvPortMalloc（Size * sizeof（double））;
        err |= （filter->wc == 0）;

        filter->sigma_point = pvPortMalloc（Size * state_dim * sizeof（double））;
        err |= （filter->sigma_point == 0）;

        Size = filter->state_dim;

        filter->sigma = pvPortMalloc（Size * sizeof（double））;
        err |= （filter->sigma == 0）;

        filter->PM = pvPortMalloc（Size * Size * sizeof（double））;
        err |= （filter->PM == 0）;

        filter->PM_save = pvPortMalloc（Size * Size * sizeof（double））;
        err |= （filter->PM == 0）;

        filter->xm = pvPortMalloc（Size * sizeof（double））;
        err |= （filter->xm == 0）;

        filter->ym = pvPortMalloc（filter->measure_dim * sizeof（double））;
        err |= （filter->ym == 0）;

        Size = 2 * filter->state_dim + 1;
        filter->khi = pvPortMalloc（Size * filter->state_dim * sizeof（double））;
        err |= （filter->khi == 0）;

        filter->khi_y = pvPortMalloc（Size * filter->measure_dim * sizeof（double））;
        err |= （filter->khi_y == 0）;

        Size = filter->measure_dim;
        filter->Pyy = pvPortMalloc（Size * Size * sizeof（double））;
        err |= （filter->Pyy == 0）;

        filter->Pxy = pvPortMalloc（Size * filter->state_dim * sizeof（double））;
        err |= （filter->Pxy == 0）;


        filter->dx = pvPortMalloc（filter->state_dim * sizeof（double））;
        err |= （filter->dx == 0）;

        filter->

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      12285  2009-09-26 09:51  UKFC源码\1\kalman.c

     文件       3460  2009-09-26 09:48  UKFC源码\1\kalman.h

     文件       2391  2009-09-26 09:55  UKFC源码\1\matrix.c

     文件       1417  2009-09-26 09:57  UKFC源码\1\matrix.h

     目录          0  2009-09-27 09:11  UKFC源码\1

     文件       3854  2009-09-26 10:36  UKFC源码\2\unscent kalman filter.cpp

     文件       2816  2009-09-26 10:42  UKFC源码\2\unscent kalman filter.h

     文件      47615  2009-09-26 11:12  UKFC源码\2\unscent kalman filter__2.cpp

     文件       6684  2009-09-26 11:14  UKFC源码\2\unscent kalman filter__2.h

     目录          0  2009-09-27 10:36  UKFC源码\2

     目录          0  2009-09-27 09:11  UKFC源码

----------- ---------  ---------- -----  ----

                80522                    11