ウイナー推定
[xs,ps,xf,pf]=wiener(y,x0,p0,f,g,h,q,r)
間隔 [t0,tf]
におけるシステム行列
=[f0,f1,...,ff]
, および fk
は
nxn行列です
=[g0,g1,...,gf]
, および
gk
は nxn 行列です
=[h0,h1,...,hf]
,および
hk
は mxn 行列です
ダイナミクスおよび観測ノイズの共分散行列
=[q0,q1,...,qf]
,
ただし,qk
は nxn 行列
=[r0,r1,...,rf]
,
ただし,rk
は mxm 行列
初期状態量推定値および誤差共分散
間隔 [t0,tf]
における観測量.
y=[y0,y1,...,yf]
, および yk
は
m次列ベクトルです
平滑化された状態推定値 xs= [xs0,xs1,...,xsf]
, および
xsk
はn次列ベクトルです
平滑化された推定値の誤差共分散 ps=[p0,p1,...,pf]
, および
pk
は nxn 行列です
平滑化された状態推定値 xf= [xf0,xf1,...,xff]
, および
xfk
はn次列ベクトルです
平滑化された推定値の誤差共分散 pf=[p0,p1,...,pf]
,
および pk
が nxn行列です
フォワード-バックワードカルマンフィルタ定式化により ウイナー推定を出力する関数
// 統計量(平均および誤差共分散)を初期化 m0=[10 10]'; p0=[100 0;0 100]; // システムを生成 f=[1.1 50.1;0 0.8]; g=[1 0;0 1]; h=[1 0;0 1]; [hi,hj]=size(h); // ノイズの統計量 q=[.01 0;0 0.01]; r=20*eye(2,2); // システム過程を初期化 rand("seed",66); rand("normal"); p0c=chol(p0); x0=m0+p0c'*rand(ones(m0)); y=h*x0+chol(r)'*rand(ones(1:hi))'; yt=y; // プロットする変数を初期化 x=x0; // ループ ft=[f]; gt=[g]; ht=[h]; qt=[q]; rt=[r]; n=10; for k=1:n // 状態変数と観測量を // 時刻 k (すなわち xk および yk) で生成 [x1,y]=system(x0,f,g,h,q,r); x=[x x1]; yt=[yt y]; x0=x1; ft=[ft f]; gt=[gt g]; ht=[ht h]; qt=[qt q]; rt=[rt r]; end // ウィナーフィルタ推定を得る [xs,ps,xf,pf]=wiener(yt,m0,p0,ft,gt,ht,qt,rt); // 結果をプロット a=min([x(1,:)-2*sqrt(ps(1,1:2:2*(n+1))),xf(1,:),xs(1,:)]); b=max([x(1,:)+2*sqrt(ps(1,1:2:2*(n+1))),xf(1,:),xs(1,:)]); c=min([x(2,:)-2*sqrt(ps(2,2:2:2*(n+1))),xf(2,:),xs(2,:)]); d=max([x(2,:)+2*sqrt(ps(2,2:2:2*(n+1))),xf(2,:),xs(2,:)]); xmargin=max([abs(a),abs(b)]); ymargin=max([abs(c),abs(d)]); a=-0.1*xmargin+a; b=.1*xmargin+b; c=-0.1*ymargin+c; d=.1*ymargin+d; // フレーム, 状態変数 (x), および推定値 (xf, および xs)をプロット scf(); plot([a a b],[d c c]); plot2d(x(1,:)',x(2,:)',[2],"000") plot2d(xf(1,:)',xf(2,:)',[2],"000") plot2d(xs(1,:)',xs(2,:)',[2],"000") // データ点をマーク (実データ: * , 推定値: o) plot2d(xs(1,:)',xs(2,:)',[-2],"000") plot2d(xf(1,:)',xf(2,:)',[-3],"000") plot2d(x(1,:)',x(2,:)',[-4],"000") | ![]() | ![]() |