May 23, 2004
カルマンフィルタ実装計画
複数のセンサデータを取得し、それをもとにカルマンフィルタをかけることによって精度のよい観測を行う、そんなルーチンを考えています。
センサデータはできるかぎり利用したいですが、一定時間間隔で取得しなければ、誤差の広がり要因が増え、精度の高い測定を行うことができなくなると思います。従って、インターバルタイマを使用してセンサの値を取得するルーチンを用意する作戦でいきます。
こうすると、センサループから計算ルーチンへの値をどのようにわたすかが問題になりますが、これにはソフトウェアFIFOのQueueを使用すればうまくいきます。Queueを一段かませることによって、取得したデータを順序よく処理することができ、古いデータをとりこぼすこともありません(Queueのサイズを適当な値に調整する必要がありますが)。カルマンフィルタ特有の、計算時間が補正を行うときと行わないときとで変わってしまう問題にも対処できます。
で、全体の構造はつぎのようにしました。ここではカルマンフィルタの組み合わせとして、精度は普通だが応答性がよい(=一定時間にたくさんデータがとれる)センサとして加速度センサ、精度は良いが応答性が悪いセンサとしてGPSを例にしています。
コードは続きをどうぞ。
/* メインルーチン */
int main(){
init(); /*初期設定を行う*/
いつでも割り込み可能にしておく();
/* メインループ */
while(true){
while(queue.isEmpty()); /* センサデータがたまるまでまつ */
加速度センサデータ = queue.shift(); /* queueにある一番古いデータを取得 */
if(GPSで補正する回?){
/* このときまでに正常なら割り込みによってのGPS受信終わっているはずだ */
if(GPS全部受信済み?){
GPSで補正 兼 GPSと加速度センサで積分();
}else{
/* なにかしらの異常でGPSのデータが取れなかったらしい */
仕方がないので通常の加速度センサで積分();
}
}else{
加速度センサの値を使って積分();
}
}
}
/* センサループ、インターバルタイマ割り込みで正確な時間間隔で呼び出してもらう */
/* 例えばインターバルタイマで0.01secごとに割り込みをいれる */
#pragma interrupt
void int_itu(){
加速度センサA/D変換開始();
while(true){
if(A/D変換終了?){break;}
}
queue.push(加速度センサデータ); /* 取得したセンサデータをqueueに登録 */
}
/* GPSについても割り込みで */
/* GPSのデータはシリアルからやってくる(1秒に1パケットとかで) */
#pragma interrupt
void int_sci(){
たとえばGPSの受信データに関する処理();
グローバルスコープのバッファにデータをためる();
}
ちなみにですが、データを取得する以外の部分はできていたりします。ここのkalman,cppがそれです。
