/*
?*?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?estima

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????4928??2017-05-02?17:23??卡爾曼濾波C代碼\KalmanFilter.c

?????文件???????1540??2017-05-02?17:26??卡爾曼濾波C代碼\KalmanFilter.h

?????目錄??????????0??2017-05-02?17:27??卡爾曼濾波C代碼

-----------?---------??----------?-----??----

?????????????????6468????????????????????3