91av视频/亚洲h视频/操亚洲美女/外国一级黄色毛片 - 国产三级三级三级三级

  • 大小: 21KB
    文件類(lèi)型: .zip
    金幣: 2
    下載: 0 次
    發(fā)布日期: 2021-06-07
  • 語(yǔ)言: 其他
  • 標(biāo)簽: imu??mpu6050??kalman??

資源簡(jiǎn)介

6軸慣性測(cè)量單元,加速度、角速度傳感器MPU6050,讀取數(shù)據(jù)并Kalman濾波器處理數(shù)據(jù)

資源截圖

代碼片段和文件信息

/*
?*?FileName?:?kalman_filter.c
?*?Author???:?xiahouzuoxin?@163.com
?*?Version??:?v1.0
?*?Date?????:?2014/9/24?20:36:51
?*?Brief????:?
?*?
?*?Copyright?(C)?MICLUSTB
?*/
?
#include?“kalman_filter.h“

/*
?*?@brief???
?*???Init?fields?of?structure?@kalman1_state.
?*???I?make?some?defaults?in?this?init?function:
?*?????A?=?1;
?*?????H?=?1;?
?*???and?@q@r?are?valued?after?prior?tests.
?*
?*???NOTES:?Please?change?AHqr?according?to?your?application.
?*
?*?@inputs??
?*???state?-?Klaman?filter?structure
?*???init_x?-?initial?x?state?value???
?*???init_p?-?initial?estimated?error?convariance
?*?@outputs?
?*?@retval??
?*/
void?kalman1_init(kalman1_state?*state?float?init_x?float?init_p)
{
????state->x?=?init_x;
????state->p?=?init_p;
????state->A?=?1;
????state->H?=?1;
????state->q?=?2e2;//10e-6;??/*?predict?noise?convariance?*/
????state->r?=?5e2;//10e-5;??/*?measure?error?convariance?*/
}

/*
?*?@brief???
?*???1?Dimension?Kalman?filter
?*?@inputs??
?*???state?-?Klaman?filter?structure
?*???z_measure?-?Measure?value
?*?@outputs?
?*?@retval??
?*???Estimated?result
?*/
float?kalman1_filter(kalman1_state?*state?float?z_measure)
{
????/*?Predict?*/
????state->x?=?state->A?*?state->x;
????state->p?=?state->A?*?state->A?*?state->p?+?state->q;??/*?p(n|n-1)=A^2*p(n-1|n-1)+q?*/

????/*?Measurement?*/
????state->gain?=?state->p?*?state->H?/?(state->p?*?state->H?*?state->H?+?state->r);
????state->x?=?state->x?+?state->gain?*?(z_measure?-?state->H?*?state->x);
????state->p?=?(1?-?state->gain?*?state->H)?*?state->p;

????return?state->x;
}

/*
?*?@brief???
?*???Init?fields?of?structure?@kalman1_state.
?*???I?make?some?defaults?in?this?init?function:
?*?????A?=?{{1?0.1}?{0?1}};
?*?????H?=?{10};?
?*???and?@q@r?are?valued?after?prior?tests.?
?*
?*???NOTES:?Please?change?AHqr?according?to?your?application.
?*
?*?@inputs??
?*?@outputs?
?*?@retval??
?*/
void?kalman2_init(kalman2_state?*state?float?*init_x?float?(*init_p)[2])
{
????state->x[0]????=?init_x[0];
????state->x[1]????=?init_x[1];
????state->p[0][0]?=?init_p[0][0];
????state->p[0][1]?=?init_p[0][1];
????state->p[1][0]?=?init_p[1][0];
????state->p[1][1]?=?init_p[1][1];
????//state->A???????=?{{1?0.1}?{0?1}};
????state->A[0][0]?=?1;
????state->A[0][1]?=?0.1;
????state->A[1][0]?=?0;
????state->A[1][1]?=?1;
????//state->H???????=?{10};
????state->H[0]????=?1;
????state->H[1]????=?0;
????//state->q???????=?{{10e-60}?{010e-6}};??/*?measure?noise?convariance?*/
????state->q[0]????=?10e-7;
????state->q[1]????=?10e-7;
????state->r???????=?10e-7;??/*?estimated?error?convariance?*/
}

/*
?*?@brief???
?*???2?Dimension?kalman?filter
?*?@inputs??
?*???state?-?Klaman?filter?structure
?*???z_measure?-?Measure?value
?*?@outputs?
?*???state->x[0]?-?Updated?state?value?Such?as?anglevelocity
?*???state->x[1]?-?Updated?state?value?Such?as?diffrence?angle?acceleration
?*???state->p????-?Updated?estimated?error?convatiance?matrix
?*?@retval??
?*???Return?value?is?equals?to?state->x[0]?so?maybe?angle?or?velo

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----
?????目錄???????????0??2014-09-25?03:46??kalman_filter-master\
?????文件?????????187??2014-09-25?03:46??kalman_filter-master\Makefile
?????文件????????2443??2014-09-25?03:46??kalman_filter-master\README.md
?????文件????????7775??2014-09-25?03:46??kalman_filter-master\fixed_test1.h
?????文件????????5790??2014-09-25?03:46??kalman_filter-master\fixed_test2.h
?????文件????????2067??2014-09-25?03:46??kalman_filter-master\free_test07.h
?????文件????????4780??2014-09-25?03:46??kalman_filter-master\kalman_filter.c
?????文件????????1495??2014-09-25?03:46??kalman_filter-master\kalman_filter.h
?????文件????????1628??2014-09-25?03:46??kalman_filter-master\plot_result.asv
?????文件????????1734??2014-09-25?03:46??kalman_filter-master\plot_result.m
?????文件???????11634??2014-09-25?03:46??kalman_filter-master\result.png
?????文件????????5101??2014-09-25?03:46??kalman_filter-master\result.txt
?????文件????????1912??2014-09-25?03:46??kalman_filter-master\test_kalman_filter.c

評(píng)論

共有 條評(píng)論

相關(guān)資源