kalm

引数

f,g,h

カレントのシステム行列

q, r

ダイナミクスの共分散行列と観測ノイズ

x0,p0

t=-1までのデータに基づくt=0における状態推定量および誤差共分散

y

カレントの観測出力

x1,p1

t=0までのデータに基づくt=1における 更新後の推定状態量および誤差共分散

x

t=0までのデータに基づくt=0における 更新後の推定状態量および誤差共分散

説明

この関数は,カルマン更新と誤差共分散計算を行います. これを行うため, 以下の状態空間モデルに基づき f, g, h を入力する必要があります:

x(k+1)=f*x(k)+g*u(k)+v(k)

y(k)=h*x(k)+w(k)

ただし,v(k) (w(k)に関する) はプロセスノイズ (観測ノイズに関する)であり, 共分散q (rに基づく)のゼロ平均ガウス白色ノイズ に起因すると仮定します.

厳密に述べると, カルマンフィルタは,現在の状態と誤差共分散の推定値を出力する 再帰的推定器です. 必要なのが前ステップの状態量推定値と現在の観測量のみであるということが 優れた点です.

アルゴリズム:

• イノベーション (出力誤差): E=y-h*x0

• 出力の誤差共分散: S=h*p0*h'+r

• カルマンゲイン: K=p0*h'*S^-1

• 状態量の推定値の修正: x=x0+K*E

• 誤差共分散の推定値の修正: p=p0-K*h*p0

• 状態量の予測: x1=f*x

• 誤差共分散の予測: p1=f*p*f'+g*q*g'

カルマンフィルタによりノイズからサイン波を抽出

// サイン波を作成
w=%pi/4; // 各周波数
T=0.1; // 周期
t=0:T:500;
signal=cos(w*t);
// ノイズを有するサイン波
v=rand(t,"normal");
y=signal+v;
// ノイズを有するサイン波をプロット
subplot(2,1,1);
plot(t,y);
xtitle("sinusoid with noise","t");
// システム
n=2; // システムの次数
f=[cos(w*T) -sin(w*T); sin(w*T) cos(w*T)];
g=0;
h=[1 0];
p0=[1000 0; 0 0];
R=1;
Q=0;
x0=zeros(n,1);
// ループを初期化
x1=x0;
p1=p0;
// カルマンフィルタ
for i=1:length(t)-1
    [x1(:,i+1),p1,x,p]=kalm(y(i),x1(:,i),p1,f,g,h,Q,R);
end
// サイン波(緑)と比較するために,結果を(赤で)プロット
subplot(2,1,2);
plot(t,signal,"color","green");
plot(t,x1(1,:),"color","red");
xtitle("Comparison between sinusoid (green) and extraction with Kalman filter (red)","t");

参照

• srkf — 平方根カルマンフィルタ
• sskf — 定常状態カルマンフィルタ