• 大小: 17KB
    文件类型: .rar
    金币: 1
    下载: 0 次
    发布日期: 2021-06-10
  • 语言: C/C++
  • 标签: kalman  ukf  C语言  

资源简介

ukf和kalma滤波用C语言实现,代码文件提供一个例子,供参考。

资源截图

代码片段和文件信息

/*
 * 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  UKF和Kalman滤波源码\1\kalman.c

     文件       3460  2009-09-26 09:48  UKF和Kalman滤波源码\1\kalman.h

     文件       2391  2009-09-26 09:55  UKF和Kalman滤波源码\1\matrix.c

     文件       1417  2009-09-26 09:57  UKF和Kalman滤波源码\1\matrix.h

     文件       3854  2009-09-26 10:36  UKF和Kalman滤波源码\2\unscent kalman filter.cpp

     文件       2816  2009-09-26 10:42  UKF和Kalman滤波源码\2\unscent kalman filter.h

     文件      47615  2009-09-26 11:12  UKF和Kalman滤波源码\2\unscent kalman filter__2.cpp

     文件       6684  2009-09-26 11:14  UKF和Kalman滤波源码\2\unscent kalman filter__2.h

     目录          0  2009-09-27 09:11  UKF和Kalman滤波源码\1

     目录          0  2009-09-27 10:36  UKF和Kalman滤波源码\2

     目录          0  2009-09-27 09:11  UKF和Kalman滤波源码

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

                80522                    11


评论

共有 条评论