項目編號：E1750

文件大?。?1K

源碼說明：帶中文注釋

開發(fā)環(huán)境：C編譯器

簡要概述：

這是一個Kalman Filter的程序，內(nèi)含狀態(tài)量為標(biāo)量和二維矢量的C程序?qū)崿F(xiàn)，詳見kalman_filter.c與kalman_filter.h。

程序使用的數(shù)據(jù)類型是float，程序測試主要在PC上和TMS320C6000的嵌入式平臺。當(dāng)然，因為是C程序，移植到其它的平臺那是特別的方便！

fixed_test1.h``fixed_test2.h以及free_test07.h包含了測試用的數(shù)據(jù)。test_kalman_filter.c只是一個包含main函數(shù)測試程序。

Example

環(huán)境要求：gcc的C編譯環(huán)境，當(dāng)然最好有Matlab環(huán)境可以非常直觀方便的看到濾波效果。

在test_kalman_filter.c中修改要使用Kalman濾波的數(shù)據(jù)，只要修改include包含即可

//#include "fixed_test1.h"

#include "fixed_test2.h"

//#include "free_test07.h"

推薦使用已經(jīng)寫好的Makefile文件進行編譯執(zhí)行，在gcc環(huán)境中，依次執(zhí)行以下命令：

make       " gcc編譯程序

make run   " 運行程序

之后數(shù)據(jù)和濾波結(jié)果會存儲到當(dāng)前目錄的result.txt文件中（第一列為原數(shù)據(jù)，第二列為濾波結(jié)果）

運行Matlab，執(zhí)行腳本plot_result.m，選擇result.txt文件即可查看濾波效果，下面為使用標(biāo)量濾波的效果，卡爾曼濾波器使突變的數(shù)據(jù)變得平滑。

Usage

實際使用時只需要將kalman_filter.c與kalman_filter.h拷貝到工程目錄中即可，調(diào)用的步驟一律按照：

kalman_init(..., init_x, init_p)
kalman_filter(..., z_measure)

本程序未包含自適應(yīng)調(diào)參，因此請根據(jù)實際應(yīng)用環(huán)境仔細(xì)調(diào)整，很重要。關(guān)于調(diào)參有很多可研究的地方，主要需要設(shè)定的參數(shù)如下：

1. init_x：待測量的初始值，如有中值一般設(shè)成中值（如陀螺儀）

2. init_p：后驗狀態(tài)估計值誤差的方差的初始值

3. q：預(yù)測（過程）噪聲方差

4. r：測量（觀測）噪聲方差。以陀螺儀為例，測試方法是：保持陀螺儀不動，統(tǒng)計一段時間內(nèi)的陀螺儀輸出數(shù)據(jù)。數(shù)據(jù)會近似正態(tài)分布，按3σ原則，取正態(tài)分布的(3σ)^2作為r的初始化值。

其中q和r參數(shù)尤為重要，一般得通過實驗測試得到。

關(guān)于Kalman濾波的詳細(xì)公式推導(dǎo)參見論文An Introduction to the Kalman Filter.

目錄│文件列表:

 └ kalman_filter

    │ fixed_test1.h

    │ fixed_test2.h

    │ free_test07.h

    │ kalman_filter.c

    │ kalman_filter.h

    │ Makefile

    │ plot_result.asv

    │ plot_result.m

    │ result.png

    │ result.txt

    └ test_kalman_filter.c