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