🚘
Moment-Based Kalman FilterのMATLABでの実装
はじめに
下記論文において，Moment-Based Kalman Filter(MKF)というものが提案されている．
これは，システムの状態を確率的に扱うことにより，精度の良い推定を行うことができ，特に，非ガウスノイズ下において他のKalman Filterよりも精度向上する模様である．
提案手法は非常に面白そうなので中身を追っていったが，公開されているc++コードを読み解くのが大変だったので，MATLABで再実装する．
実装にあたっては確率変数に関するモーメント計算が必要になるが，その計算については下記記事でまとめている．
なお，最終的な実装は下記
本記事でやること・やらないこと
やること
- Moment-based Kalman Filter(MKF)の実装
やらないこと
- 実装にあたり必要な論文の式展開と補完
- 理論的背景（そもそもなぜ提案手法の式が出るのか）の説明
※当初は式展開も記載しようとしたが，量が多いのと式の大半が論文の写しになってしまうので，今回は記載しないものとする．
（もし必要に駆られたりやる気がでたら追加するかも…）
MKFのMATLAB実装
対象のモデル
本記事では，論文中でも用いられているUTIASデータセットを用いて検証を行う．
上記データセットは二輪車両であり，並進速度
また，モーションキャプチャにより，位置と姿勢
準備
確率変数計算のクラス化
確率変数のモーメント計算を楽に呼び出すため，のようにクラスを作成する．
参考：独立でない変数の混合三角モーメントの計算の色々な例
1変数の場合と，3変数の混合三角モーメントの場合でそれぞれ作成する．
１変数の場合のモーメント計算
% compute E[f(x)],x~N(mu sigma2)
% input: mu,sigma (scalar)
% refference:https://arxiv.org/pdf/2101.12490.pdf
classdef GaussianTPM
properties
mu
sigma2
end
methods
function obj = GaussianTPM(mu,sigma2)
if nargin == 2
obj.mu = mu;
obj.sigma2 = sigma2;
end
end
function e = X(obj)
e = obj.mu;
end
function e = XX(obj)
% E[x^2]
e = obj.sigma2+obj.mu^2;
end
function e = C(obj)
% E[cosx]
e = cos(obj.mu)*exp(-0.5*obj.sigma2);
end
function e = S(obj)
% E[sinx]
e = sin(obj.mu)*exp(-0.5*obj.sigma2);
end
function e = XC(obj)
% E[xcosx]
e = (obj.mu*cos(obj.mu)-obj.sigma2*sin(obj.mu)) * exp(-0.5*obj.sigma2);
end
function e = XS(obj)
% E[xsinx]
e = (obj.mu*sin(obj.mu)+obj.sigma2*cos(obj.mu)) * exp(-0.5*obj.sigma2);
end
function e = XXC(obj)
% E[x^2 cosx]
e = ((obj.sigma2+obj.mu^2-obj.sigma2^2)*cos(obj.mu)-2*obj.mu*obj.sigma2*sin(obj.mu))*exp(-0.5*obj.sigma2);
end
function e = XXS(obj)
% E[x^2 sinx]
e =((obj.sigma2+obj.mu^2-obj.sigma2^2)*sin(obj.mu)+2*obj.mu*obj.sigma2*cos(obj.mu))*exp(-0.5*obj.sigma2);
end
function e = CS(obj)
% E[cosxsinx]
e = 0.5*sin(2*obj.mu)*exp(-0.5*4*obj.sigma2);
end
function e = CC(obj)
% E[cos^2x]
e = 0.5*(cos(2*obj.mu)*exp(-0.5*4*obj.sigma2)+1);
end
function e = SS(obj)
% E[sin^2x]
e = 0.5*(1-cos(2*obj.mu)*exp(-0.5*4*obj.sigma2));
end
end
end
3変数の場合のモーメント計算
% compute E[f(x)],x~N(mu sigma2)
% input: mu(3-dimention vector),sigma (3*3 matrix)
classdef GaussianMixedTPM3d < GaussianTPM
properties
Mu,Sigma2
x,y,th
T,Lambda
Z31,Z32,Z33
end
methods
function obj = GaussianMixedTPM3d(Mu,Sigma2)
if nargin == 2
if size(Mu,1) ~= 3
error('Not 3-dimension, check dimension of mu or sigma')
end
obj.Mu = Mu;
obj.Sigma2 = Sigma2;
[obj.T,obj.Lambda] = eig(Sigma2);
T31 = obj.T(3,1);
T32 = obj.T(3,2);
T33 = obj.T(3,3);
mu_y = obj.T'*obj.Mu;
mu_y1 = mu_y(1);
mu_y2 = mu_y(2);
mu_y3 = mu_y(3);
lambda1 = obj.Lambda(1,1);
lambda2 = obj.Lambda(2,2);
lambda3 = obj.Lambda(3,3);
% 確率分布のクラス定義
obj.Z31 = GaussianTPM(T31*mu_y1,(T31^2)*lambda1);
obj.Z32 = GaussianTPM(T32*mu_y2,(T32^2)*lambda2);
obj.Z33 = GaussianTPM(T33*mu_y3,(T33^2)*lambda3);
obj.x = GaussianTPM(obj.Mu(1),obj.Sigma2(1,1));
obj.y = GaussianTPM(obj.Mu(2),obj.Sigma2(2,2));
obj.th = GaussianTPM(obj.Mu(3),obj.Sigma2(3,3));
end
end
function e = X(obj)
e = obj.x.X;
end
function e = Y(obj)
e = obj.y.X;
end
function e = Th(obj)
e = obj.th.X;
end
function e = C(obj)
e = obj.th.C;
end
function e = S(obj)
e = obj.th.S;
end
function e = CS(obj)
% e = 0.5*sin(2*obj.Mu(3))*exp(-0.5*4*obj.Sigma2(3,3));
e = obj.th.CS;
end
function e = SC(obj)
e = obj.th.CS;
end
function e = CC(obj)
% e = 0.5*(cos(2*obj.Mu(3))*exp(-0.5*4*obj.Sigma2(3,3))+1);
e = obj.th.CC;
end
function e = SS(obj)
% e = 0.5*(1-cos(2*obj.Mu(3))*exp(-0.5*4*obj.Sigma2(3,3)));
e = obj.th.SS;
end
function e = XX(obj)
% E[x^2]
% e = obj.Sigma2(1,1)+obj.Mu(1)^2;
e = obj.x.XX;
end
function e = YY(obj)
% E[y^2]
% e = obj.Sigma2(2,2)+obj.Mu(2)^2;
e = obj.y.XX;
end
function e = ThTh(obj)
% E[y^2]
% e = obj.Sigma2(3,3)+obj.Mu(3)^2;
e = obj.th.XX;
end
function e = ThC(obj)
% e = (obj.Mu(3)*cos(obj.Mu(3))-obj.Sigma2(3,3)*sin(obj.Mu(3))) * exp(-0.5*obj.Sigma2(3,3));
e = obj.th.XC;
end
function e = ThS(obj)
% e = (obj.Mu(3)*sin(obj.Mu(3))+obj.Sigma2(3,3)*cos(obj.Mu(3))) * exp(-0.5*obj.Sigma2(3,3));
e = obj.th.XS;
end
function e = XY(obj)
if obj.Sigma2(1,2)==0
e = obj.x.X*obj.y.X;
else
[T11,T12,T13,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*((T21/T31)*z31.XX+(T22/T32)*z31.X*z32.X+(T23/T33)*z31.X*z33.X)...
+(T12/T32)*((T21/T31)*z31.X*z32.X+(T22/T32)*z32.XX+(T23/T33)*z32.X*z33.X)...
+(T13/T33)*((T21/T31)*z33.X*z31.X+(T22/T32)*z33.X*z32.X+(T23/T33)*z33.XX);
end
end
function e = XTh(obj)
if obj.Sigma2(1,3)==0
e = obj.x.X*obj.th.X;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(z31.XX+z31.X*z32.X+z31.X*z33.X)...
+(T12/T32)*(z31.X*z32.X+z32.XX+z32.X*z33.X)...
+(T13/T33)*(z33.X*z31.X+z33.X*z32.X+z33.XX);
end
end
function e = YTh(obj)
if obj.Sigma2(2,3)==0
e = obj.y.X*obj.th.X;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)*(z31.XX+z31.X*z32.X+z31.X*z33.X)...
+(T22/T32)*(z31.X*z32.X+z32.XX+z32.X*z33.X)...
+(T23/T33)*(z33.X*z31.X+z33.X*z32.X+z33.XX);
end
end
function e = XC(obj)
% calc E[(x*cos(theta))]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.C;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(z31.XC*z32.C*z33.C-z31.XC*z32.S*z33.S-z31.XS*z32.C*z33.S-z31.XS*z32.S*z33.C)...
+(T12/T32)*(z31.C*z32.XC*z33.C-z31.C*z32.XS*z33.S-z31.S*z32.XC*z33.S-z31.S*z32.XS*z33.C)...
+(T13/T33)*(z31.C*z32.C*z33.XC-z31.C*z32.S*z33.XS-z31.S*z32.C*z33.XS-z31.S*z32.S*z33.XC);
end
end
function e = XS(obj)
% calc E[(x*sin(theta))]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.S;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(-z31.XS*z32.S*z33.S+z31.XS*z32.C*z33.C+z31.XC*z32.S*z33.C+z31.XC*z32.C*z33.S)...
+(T12/T32)*(-z31.S*z32.XS*z33.S+z31.S*z32.XC*z33.C+z31.C*z32.XS*z33.C+z31.C*z32.XC*z33.S)...
+(T13/T33)*(-z31.S*z32.S*z33.XS+z31.S*z32.C*z33.XC+z31.C*z32.S*z33.XC+z31.C*z32.C*z33.XS);
end
end
function e = YC(obj)
% calc E[(y*cos(theta))]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.C;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)*(z31.XC*z32.C*z33.C-z31.XC*z32.S*z33.S-z31.XS*z32.C*z33.S-z31.XS*z32.S*z33.C)...
+(T22/T32)*(z31.C*z32.XC*z33.C-z31.C*z32.XS*z33.S-z31.S*z32.XC*z33.S-z31.S*z32.XS*z33.C)...
+(T23/T33)*(z31.C*z32.C*z33.XC-z31.C*z32.S*z33.XS-z31.S*z32.C*z33.XS-z31.S*z32.S*z33.XC);
end
end
function e = YS(obj)
% calc E[(y*sin(theta))]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.S;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)*(-z31.XS*z32.S*z33.S+z31.XS*z32.C*z33.C+z31.XC*z32.S*z33.C+z31.XC*z32.C*z33.S)...
+(T22/T32)*(-z31.S*z32.XS*z33.S+z31.S*z32.XC*z33.C+z31.C*z32.XS*z33.C+z31.C*z32.XC*z33.S)...
+(T23/T33)*(-z31.S*z32.S*z33.XS+z31.S*z32.C*z33.XC+z31.C*z32.S*z33.XC+z31.C*z32.C*z33.XS);
end
end
function e = XXC(obj)
% calc E[(x*x*cos(theta))]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.XX*obj.th.C;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)^2*(z31.XXC*z32.C*z33.C-z31.XXC*z32.S*z33.S-z31.XXS*z32.C*z33.S-z31.XXS*z32.S*z33.C)...
+(T12/T32)^2*(z31.C*z32.XXC*z33.C-z31.C*z32.XXS*z33.S-z31.S*z32.XXC*z33.S-z31.S*z32.XXS*z33.C)...
+(T13/T33)^2*(z31.C*z32.C*z33.XXC-z31.C*z32.S*z33.XXS-z31.S*z32.C*z33.XXS-z31.S*z32.S*z33.XXC)...
+2*(T11/T31)*(T12/T32)*(z31.XC*z32.XC*z33.C-z31.XC*z32.XS*z33.S-z31.XS*z32.XC*z33.S-z31.XS*z32.XS*z33.C)...
+2*(T12/T32)*(T13/T33)*(z31.C*z32.XC*z33.XC-z31.C*z32.XS*z33.XS-z31.S*z32.XC*z33.XS-z31.S*z32.XS*z33.XC)...
+2*(T13/T33)*(T11/T31)*(z31.XC*z32.C*z33.XC-z31.XC*z32.S*z33.XS-z31.XS*z32.C*z33.XS-z31.XS*z32.S*z33.XC);
end
end
function e = XCC(obj)
% calc E[(x**cos^2(theta))]=E[x**(cos(2theta)+1)/2]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.CC;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/4)*(tmp.X+tmp.XC);
end
end
function e = XSS(obj)
% calc E[(x*x*sin^2(theta))]=E[x*(1-cos(2theta))/2]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.SS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/4)*(tmp.X-tmp.XC);
end
end
function e = XCS(obj)
% calc E[(x*cos(theta)*sin(theta)]=E[x*sin(2theta)/2]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.CS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/4)*(tmp.XS);
end
end
function e = XSC(obj)
e = obj.XCS;
end
function e = XXCS(obj)
% calc E[(xx*cos(theta)*sin(theta)]=E[xx*sin(2theta)/2]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.XX*obj.th.CS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.XXS);
end
end
function e = XXSC(obj)
e = obj.XXCS;
end
function e = YCC(obj)
% calc E[(y*cos^2(theta))]=E[y*(cos(2theta)+1)/2]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.CC;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/4)*(tmp.Y+tmp.YC);
end
end
function e = YSS(obj)
% calc E[(y*sin^2(theta))]=E[y*(1-cos(2theta))/2]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.SS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/4)*(tmp.Y-tmp.YC);
end
end
function e = YCS(obj)
% calc E[(y*cos(theta)*sin(theta)]=E[y*sin(2theta)/2]
if obj.Sigma2(2,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.CS;
else
%y=2y,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/4)*(tmp.YS);
end
end
function e = YSC(obj)
e = obj.YCS;
end
function e = YYCS(obj)
% calc E[(yy*cos(theta)*sin(theta)]=E[yy*sin(2theta)/2]
if obj.Sigma2(2,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.XX*obj.th.CS;
else
%y=2y,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.YYS);
end
end
function e = YYSC(obj)
e = obj.YYCS;
end
function e = XYCS(obj)
% calc E[(x*y*cos(theta)*sin(theta)]=E[x*y*sin(2theta)/2]
if isdiag(obj.Sigma2) == true %Sigma2が対角→x,y,thetaが独立
e = obj.x.X*obj.y.X*obj.th.CS;
else
%x=2X,y=2y,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.XYS);
end
end
function e = XYSC(obj)
e = obj.XYCS;
end
function e = XYCC(obj)
% calc E[(xy*cos^2(theta))]=E[xy*(cos(2theta)+1)/2]
if isdiag(obj.Sigma2) == true %Sigma2が対角→x,y,thetaが独立
e = obj.x.X*obj.y.X*obj.th.CC;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.XY+tmp.XYC);
end
end
function e = XYSS(obj)
% calc E[(xy*sin^2(theta))]=E[xy*(1-cos(2theta))/2]
if isdiag(obj.Sigma2) == true %Sigma2が対角→x,y,thetaが独立
e = obj.x.X*obj.y.X*obj.th.SS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.XY-tmp.XYC);
end
end
function e = XXCC(obj)
% calc E[(x*x*cos^2(theta))]=E[x*x*(cos(2theta)+1)/2]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.XX*obj.th.CC;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.XX+tmp.XXC);
end
end
function e = XXSS(obj)
% calc E[(x*x*sin^2(theta))]=E[x*x*(1-cos(2theta))/2]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.XX*obj.th.SS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.XX-tmp.XXC);
end
end
function e = YYCC(obj)
% calc E[(y*y*cos^2(theta))]=E[y*y*(cos(2theta)+1)/2]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.XX*obj.th.CC;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.YY+tmp.YYC);
end
end
function e = YYSS(obj)
% calc E[(y*y*sin^2(theta))]=E[y*y*(1-cos(2theta))/2]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.XX*obj.th.SS;
else
%x=2x,theta=2thetaと置きなおして再帰的に呼び出す
tmp = GaussianMixedTPM3d(2*obj.Mu,4*obj.Sigma2);
e = (1/8)*(tmp.YY-tmp.YYC);
end
end
function e = XXS(obj)
% calc E[(x*x*sin(theta))]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.XX*obj.th.S;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)^2*(-z31.XXS*z32.S*z33.S+z31.XXS*z32.C*z33.C+z31.XXC*z32.S*z33.C+z31.XXC*z32.C*z33.S)...
+(T12/T32)^2*(-z31.S*z32.XXS*z33.S+z31.S*z32.XXC*z33.C+z31.C*z32.XXS*z33.C+z31.C*z32.XXC*z33.S)...
+(T13/T33)^2*(-z31.S*z32.S*z33.XXS+z31.S*z32.C*z33.XXC+z31.C*z32.S*z33.XXC+z31.C*z32.C*z33.XXS)...
+2*(T11/T31)*(T12/T32)*(-z31.XS*z32.XS*z33.S+z31.XS*z32.XC*z33.C+z31.XC*z32.XS*z33.C+z31.XC*z32.XC*z33.S)...
+2*(T12/T32)*(T13/T33)*(-z31.S*z32.XS*z33.XS+z31.S*z32.XC*z33.XC+z31.C*z32.XS*z33.XC+z31.C*z32.XC*z33.XS)...
+2*(T13/T33)*(T11/T31)*(-z31.XS*z32.S*z33.XS+z31.XS*z32.C*z33.XC+z31.XC*z32.S*z33.XC+z31.XC*z32.C*z33.XS);
end
end
function e = YYC(obj)
% calc E[(y*y*cos(theta))]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.XX*obj.th.C;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)^2*(z31.XXC*z32.C*z33.C-z31.XXC*z32.S*z33.S-z31.XXS*z32.C*z33.S-z31.XXS*z32.S*z33.C)...
+(T22/T32)^2*(z31.C*z32.XXC*z33.C-z31.C*z32.XXS*z33.S-z31.S*z32.XXC*z33.S-z31.S*z32.XXS*z33.C)...
+(T23/T33)^2*(z31.C*z32.C*z33.XXC-z31.C*z32.S*z33.XXS-z31.S*z32.C*z33.XXS-z31.S*z32.S*z33.XXC)...
+2*(T21/T31)*(T22/T32)*(z31.XC*z32.XC*z33.C-z31.XC*z32.XS*z33.S-z31.XS*z32.XC*z33.S-z31.XS*z32.XS*z33.C)...
+2*(T22/T32)*(T23/T33)*(z31.C*z32.XC*z33.XC-z31.C*z32.XS*z33.XS-z31.S*z32.XC*z33.XS-z31.S*z32.XS*z33.XC)...
+2*(T23/T33)*(T21/T31)*(z31.XC*z32.C*z33.XC-z31.XC*z32.S*z33.XS-z31.XS*z32.C*z33.XS-z31.XS*z32.S*z33.XC);
end
end
function e = YYS(obj)
% calc E[(y*y*sin(theta))]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.XX*obj.th.S;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)^2*(-z31.XXS*z32.S*z33.S+z31.XXS*z32.C*z33.C+z31.XXC*z32.S*z33.C+z31.XXC*z32.C*z33.S)...
+(T22/T32)^2*(-z31.S*z32.XXS*z33.S+z31.S*z32.XXC*z33.C+z31.C*z32.XXS*z33.C+z31.C*z32.XXC*z33.S)...
+(T23/T33)^2*(-z31.S*z32.S*z33.XXS+z31.S*z32.C*z33.XXC+z31.C*z32.S*z33.XXC+z31.C*z32.C*z33.XXS)...
+2*(T21/T31)*(T22/T32)*(-z31.XS*z32.XS*z33.S+z31.XS*z32.XC*z33.C+z31.XC*z32.XS*z33.C+z31.XC*z32.XC*z33.S)...
+2*(T22/T32)*(T23/T33)*(-z31.S*z32.XS*z33.XS+z31.S*z32.XC*z33.XC+z31.C*z32.XS*z33.XC+z31.C*z32.XC*z33.XS)...
+2*(T23/T33)*(T21/T31)*(-z31.XS*z32.S*z33.XS+z31.XS*z32.C*z33.XC+z31.XC*z32.S*z33.XC+z31.XC*z32.C*z33.XS);
end
end
function e = XYC(obj)
% calc E[(x*y*cos(theta))]
if isdiag(obj.Sigma2) == true %Sigma2が対角→x,y,thetaが独立
e = obj.x.X*obj.y.X*obj.th.C;
else
[T11,T12,T13,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(T21/T31)*(z31.XXC*z32.C*z33.C-z31.XXC*z32.S*z33.S-z31.XXS*z32.C*z33.S-z31.XXS*z32.S*z33.C)...
+(T12/T32)*(T22/T32)*(z31.C*z32.XXC*z33.C-z31.C*z32.XXS*z33.S-z31.S*z32.XXC*z33.S-z31.S*z32.XXS*z33.C)...
+(T13/T33)*(T23/T33)*(z31.C*z32.C*z33.XXC-z31.C*z32.S*z33.XXS-z31.S*z32.C*z33.XXS-z31.S*z32.S*z33.XXC)...
+((T11/T31)*(T22/T32)+(T12/T32)*(T21/T31))*(z31.XC*z32.XC*z33.C-z31.XC*z32.XS*z33.S-z31.XS*z32.XC*z33.S-z31.XS*z32.XS*z33.C)...
+((T12/T32)*(T23/T33)+(T13/T33)*(T22/T32))*(z31.C*z32.XC*z33.XC-z31.C*z32.XS*z33.XS-z31.S*z32.XC*z33.XS-z31.S*z32.XS*z33.XC)...
+((T13/T33)*(T21/T31)+(T11/T31)*(T23/T33))*(z31.XC*z32.C*z33.XC-z31.XC*z32.S*z33.XS-z31.XS*z32.C*z33.XS-z31.XS*z32.S*z33.XC);
end
end
function e = XYS(obj)
% calc E[(x*y*sin(theta))]
if isdiag(obj.Sigma2) == true %Sigma2が対角→x,y,thetaが独立
e = obj.x.X*obj.y.X*obj.th.S;
else
[T11,T12,T13,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(T21/T31)*(-z31.XXS*z32.S*z33.S+z31.XXS*z32.C*z33.C+z31.XXC*z32.S*z33.C+z31.XXC*z32.C*z33.S)...
+(T12/T32)*(T22/T32)*(-z31.S*z32.XXS*z33.S+z31.S*z32.XXC*z33.C+z31.C*z32.XXS*z33.C+z31.C*z32.XXC*z33.S)...
+(T13/T33)*(T23/T33)*(-z31.S*z32.S*z33.XXS+z31.S*z32.C*z33.XXC+z31.C*z32.S*z33.XXC+z31.C*z32.C*z33.XXS)...
+((T11/T31)*(T22/T32)+(T12/T32)*(T21/T31))*(-z31.XS*z32.XS*z33.S+z31.XS*z32.XC*z33.C+z31.XC*z32.XS*z33.C+z31.XC*z32.XC*z33.S)...
+((T12/T32)*(T23/T33)+(T13/T33)*(T22/T32))*(-z31.S*z32.XS*z33.XS+z31.S*z32.XC*z33.XC+z31.C*z32.XS*z33.XC+z31.C*z32.XC*z33.XS)...
+((T13/T33)*(T21/T31)+(T11/T31)*(T23/T33))*(-z31.XS*z32.S*z33.XS+z31.XS*z32.C*z33.XC+z31.XC*z32.S*z33.XC+z31.XC*z32.C*z33.XS);
end
end
function e=XThC(obj)
% calc E[(x*theta*cos(theta))]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.XC;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(z31.XXC*z32.C*z33.C-z31.XXC*z32.S*z33.S-z31.XXS*z32.C*z33.S-z31.XXS*z32.S*z33.C)...
+(T12/T32)*(z31.C*z32.XXC*z33.C-z31.C*z32.XXS*z33.S-z31.S*z32.XXC*z33.S-z31.S*z32.XXS*z33.C)...
+(T13/T33)*(z31.C*z32.C*z33.XXC-z31.C*z32.S*z33.XXS-z31.S*z32.C*z33.XXS-z31.S*z32.S*z33.XXC)...
+((T11/T31)+(T12/T32))*(z31.XC*z32.XC*z33.C-z31.XC*z32.XS*z33.S-z31.XS*z32.XC*z33.S-z31.XS*z32.XS*z33.C)...
+((T12/T32)+(T13/T33))*(z31.C*z32.XC*z33.XC-z31.C*z32.XS*z33.XS-z31.S*z32.XC*z33.XS-z31.S*z32.XS*z33.XC)...
+((T11/T31)+(T13/T33))*(z31.XC*z32.C*z33.XC-z31.XC*z32.S*z33.XS-z31.XS*z32.C*z33.XS-z31.XS*z32.S*z33.XC);
end
end
function e=XThS(obj)
% calc E[(x*theta*sin(theta))]
if obj.Sigma2(1,3) == 0 % xとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.x.X*obj.th.XS;
else
[T11,T12,T13,~,~,~,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T11/T31)*(-z31.XXS*z32.S*z33.S+z31.XXS*z32.C*z33.C+z31.XXC*z32.S*z33.C+z31.XXC*z32.C*z33.S)...
+(T12/T32)*(-z31.S*z32.XXS*z33.S+z31.S*z32.XXC*z33.C+z31.C*z32.XXS*z33.C+z31.C*z32.XXC*z33.S)...
+(T13/T33)*(-z31.S*z32.S*z33.XXS+z31.S*z32.C*z33.XXC+z31.C*z32.S*z33.XXC+z31.C*z32.C*z33.XXS)...
+((T11/T31)+(T12/T32))*(-z31.XS*z32.XS*z33.S+z31.XS*z32.XC*z33.C+z31.XC*z32.XS*z33.C+z31.XC*z32.XC*z33.S)...
+((T12/T32)+(T13/T33))*(-z31.S*z32.XS*z33.XS+z31.S*z32.XC*z33.XC+z31.C*z32.XS*z33.XC+z31.C*z32.XC*z33.XS)...
+((T11/T31)+(T13/T33))*(-z31.XS*z32.S*z33.XS+z31.XS*z32.C*z33.XC+z31.XC*z32.S*z33.XC+z31.XC*z32.C*z33.XS);
end
end
function e=YThC(obj)
% calc E[(y*theta*cos(theta))]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.XC;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)*(z31.XXC*z32.C*z33.C-z31.XXC*z32.S*z33.S-z31.XXS*z32.C*z33.S-z31.XXS*z32.S*z33.C)...
+(T22/T32)*(z31.C*z32.XXC*z33.C-z31.C*z32.XXS*z33.S-z31.S*z32.XXC*z33.S-z31.S*z32.XXS*z33.C)...
+(T23/T33)*(z31.C*z32.C*z33.XXC-z31.C*z32.S*z33.XXS-z31.S*z32.C*z33.XXS-z31.S*z32.S*z33.XXC)...
+((T21/T31)+(T22/T32))*(z31.XC*z32.XC*z33.C-z31.XC*z32.XS*z33.S-z31.XS*z32.XC*z33.S-z31.XS*z32.XS*z33.C)...
+((T22/T32)+(T23/T33))*(z31.C*z32.XC*z33.XC-z31.C*z32.XS*z33.XS-z31.S*z32.XC*z33.XS-z31.S*z32.XS*z33.XC)...
+((T21/T31)+(T23/T33))*(z31.XC*z32.C*z33.XC-z31.XC*z32.S*z33.XS-z31.XS*z32.C*z33.XS-z31.XS*z32.S*z33.XC);
end
end
function e=YThS(obj)
% calc E[(y*theta*sin(theta))]
if obj.Sigma2(2,3) == 0 % yとθの共分散が0→独立なのでそれぞれの確率モーメントの積で返す
e = obj.y.X*obj.th.XS;
else
[~,~,~,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj);
e = (T21/T31)*(-z31.XXS*z32.S*z33.S+z31.XXS*z32.C*z33.C+z31.XXC*z32.S*z33.C+z31.XXC*z32.C*z33.S)...
+(T22/T32)*(-z31.S*z32.XXS*z33.S+z31.S*z32.XXC*z33.C+z31.C*z32.XXS*z33.C+z31.C*z32.XXC*z33.S)...
+(T23/T33)*(-z31.S*z32.S*z33.XXS+z31.S*z32.C*z33.XXC+z31.C*z32.S*z33.XXC+z31.C*z32.C*z33.XXS)...
+((T21/T31)+(T22/T32))*(-z31.XS*z32.XS*z33.S+z31.XS*z32.XC*z33.C+z31.XC*z32.XS*z33.C+z31.XC*z32.XC*z33.S)...
+((T22/T32)+(T23/T33))*(-z31.S*z32.XS*z33.XS+z31.S*z32.XC*z33.XC+z31.C*z32.XS*z33.XC+z31.C*z32.XC*z33.XS)...
+((T21/T31)+(T23/T33))*(-z31.XS*z32.S*z33.XS+z31.XS*z32.C*z33.XC+z31.XC*z32.S*z33.XC+z31.XC*z32.C*z33.XS);
end
end
function [T11,T12,T13,T21,T22,T23,T31,T32,T33,z31,z32,z33] = variableExpansion(obj)
T11 = obj.T(1,1);
T12 = obj.T(1,2);
T13 = obj.T(1,3);
T21 = obj.T(2,1);
T22 = obj.T(2,2);
T23 = obj.T(2,3);
T31 = obj.T(3,1);
T32 = obj.T(3,2);
T33 = obj.T(3,3);
z31 = obj.Z31;
z32 = obj.Z32;
z33 = obj.Z33;
end
end
end
検証用データの作成
上述したUTIASデータセットを使用して検証する．
検証用データ作成の手続き
モーションキャプチャによる座標データ，オドメトリ等のデータがそれぞれ別ファイルに格納されているため，それぞれ読み込む．
後で結合させるため，読み込んだファイルをtimetable形式に変換する．
% 読み込むフォルダとロボット番号を指定する．
datasetFolder="MRCLAM_Dataset1";
robotNumber = "Robot1";
% true data
% モーションキャプチャで取得した座標を真値とする
groundTruthRaw = importdata(strcat(datasetFolder,"/",robotNumber,"_Groundtruth.dat"));
groundTruth=array2timetable(groundTruthRaw.data(:,2:end), ...
"RowTimes",seconds(groundTruthRaw.data(:,1)), ...
"VariableNames",{'trueX','trueY','trueTh'});
% odometry data
odomRaw = importdata(strcat(datasetFolder,"/",robotNumber,"_Odometry.dat"));
odom = array2timetable(odomRaw.data(:,2:3), ...
"RowTimes",seconds(odomRaw.data(:,1)), ...
"VariableNames",{'vel','angVel'});
% measurement data
measurementRaw = importdata(strcat(datasetFolder,"/",robotNumber,"_Measurement.dat"));
measurement = array2timetable(measurementRaw.data(:,2:4), ...
"RowTimes",seconds(measurementRaw.data(:,1)), ...
"VariableNames",{'barcode','range','bearing'});
landmark_groundTruth = importdata(strcat(datasetFolder,"/","Landmark_Groundtruth.dat"));
landmark_groundTruth = array2table(landmark_groundTruth.data,"VariableNames",{'pole','poleX','poleY','x_std','y_std'});
barcode = readtable(strcat(datasetFolder,"/","Barcodes.dat"));
barcode = renamevars(barcode,[1,2],{'pole','barcode'});
% poleのバーコードの対応が間違えているので修正
barcode.pole(11) = 17;
barcode.pole(17) = 11;
% measurementのデータにbarcodeとlandmarkの座標を対応させる
measurement = innerjoin(measurement,barcode);
measurement = innerjoin(measurement,landmark_groundTruth);
measurement = sortrows(measurement);
上記で読み込んだデータをすべて結合させる．
その後，スタート時間を０にし，サンプリング時間dtでリサンプリングする．
% すべてのデータを結合
allData = synchronize(groundTruth,measurement,odom,"union",'previous');
allData = rmmissing(allData); %NaNの行を削除
allData.Time = allData.Time-allData.Time(1) %スタート時間を0にする
dt = 0.1;
allData = retime(allData,'regular','previous','TimeStep',seconds(dt))
このallDataを検証に使用する．
MATLABでの実装
はじめに，
参照しているポールの座標とモーションキャプチャでの値から
rnoize = normrnd(1,0.01,[size(allData,1),1]); %観測値rのノイズ
phinoize = normrnd(0,0.0007,[size(allData,1),1]); %観測値φのノイズ
tx = allData.trueX; %モーションキャプチャで取得したx座標
ty = allData.trueY; %モーションキャプチャで取得したy座標
px = allData.poleX; %参照しているポールのx座標
py = allData.poleY; %参照しているポールのy座標
phi = atan2(py-ty,px-tx)-(allData.trueTh) + phinoize;%観測値φの計算(式16)＋ノイズ
phi = unwrap(phi); %角度が2pi以上変化したものを平行移動する
r = sqrt((tx-px).^2+(ty-py).^2) .* rnoize;%観測値r＋ノイズ
計算ステップ数と初期値，初期の共分散を設定する．
steps = 5000; %計算ステップ
result = zeros(steps,3); %計算結果の格納
% 初期値の設定(allDataの1個目)
x = [allData.trueX(1);allData.trueY(1);allData.trueTh(1)];
sigma_x = [0.01^2,0,0;0,0.01^2,0;0,0,0.01^2];
result(1,:) = x';
そして，各ステップで自己位置と共分散の推定値を計算する．
forの中身がMKFのMATLAB実装となる．
なお，eq11などは論文中の式番号に対応する．
for i=2:steps
% オドメトリ―の速度と角速度にノイズを追加して確率変数とする
% ノイズの分布はeq.21
v = GaussianTPM(allData.vel(i-1),0.01);
u = GaussianTPM(allData.angVel(i-1),1.0);
% Step1
x_app = GaussianMixedTPM3d(x,sigma_x);
% Step2
%eq11,eq15
x_p = zeros(3,1);
x_p(1) = x_app.X + v.X*x_app.C*dt;
x_p(2) = x_app.Y + v.X*x_app.S*dt;
x_p(3) = x_app.Th + u.X*dt;
%eq11
sigma_p = zeros(3,3);
sigma_p(1,1) = x_app.XX + 2*v.X*dt*x_app.XC + v.XX*dt^2*x_app.CC;
sigma_p(1,2) = x_app.XY+v.X*dt*x_app.XS + v.X*dt*x_app.YC+v.XX*dt^2*x_app.CS;
sigma_p(1,3) = x_app.XTh + u.X*dt*x_app.X + v.X*dt*x_app.ThC + u.X*v.X*dt^2*x_app.C;
sigma_p(2,1) = sigma_p(1,2);
sigma_p(2,2) = x_app.YY + 2*v.X*dt*x_app.YS + v.XX*dt^2*x_app.SS;
sigma_p(2,3) = x_app.YTh + u.X*dt*x_app.Y + v.X*dt*x_app.ThS + u.X*v.X*dt^2*x_app.S;
sigma_p(3,1) = sigma_p(1,3);
sigma_p(3,2) = sigma_p(2,3);
sigma_p(3,3) = x_app.ThTh + 2*u.X*dt*x_app.Th + u.XX*dt^2;
sigma_p = sigma_p-x_p*x_p';
%Step3
% eq12
x_papp = GaussianMixedTPM3d(x_p,sigma_p);
%Step4
% ノイズの設定
vr = GaussianTPM(1.0,0.01);
vphi = GaussianTPM(0.0,0.0007);
% calc yhat.
% eq13
y = zeros(2,1);
px = allData.poleX(i);
py = allData.poleY(i);
e_ha = px*x_papp.C-x_papp.XC + py*x_papp.S-x_papp.YS;
e_hb = py*x_papp.C-x_papp.YC - px*x_papp.S+x_papp.XS;
y(1) = e_ha*vr.X*vphi.C - e_hb*vr.X*vphi.S;
y(2) = e_hb*vr.X*vphi.C + e_ha*vr.X*vphi.S;
% calc sigma_yy
% eq13
sigma_yy = zeros(2,2);
e_ha2 = (px^2*x_papp.CC-2*px*x_papp.XCC+x_papp.XXCC)...
+ (py^2*x_papp.SS-2*py*x_papp.YSS+x_papp.YYSS)...
+ 2*(px*py*x_papp.CS-px*x_papp.YCS-py*x_papp.XCS+x_papp.XYCS);
e_hb2 = (px^2*x_papp.SS-2*px*x_papp.XSS+x_papp.XXSS)...
+ (py^2*x_papp.CC-2*py*x_papp.YCC+x_papp.YYCC)...
- 2*(px*py*x_papp.CS-px*x_papp.YCS-py*x_papp.XCS+x_papp.XYCS);
e_hahb = (px*py*x_papp.CC-px*x_papp.YCC-py*x_papp.XCC+x_papp.XYCC)...
- (px^2*x_papp.CS-2*px*x_papp.XCS+x_papp.XXCS)...
+ (py^2*x_papp.CS-2*py*x_papp.YCS+x_papp.YYCS)...
- (px*py*x_papp.SS-px*x_papp.YSS-py*x_papp.XSS+x_papp.XYSS);
sigma_yy(1,1) = e_ha2*vr.XX*vphi.CC - 2*e_hahb*vr.XX*vphi.CS + e_hb2*vr.XX*vphi.SS;
sigma_yy(2,2) = e_hb2*vr.XX*vphi.CC + 2*e_hahb*vr.XX*vphi.CS + e_ha2*vr.XX*vphi.SS;
sigma_yy(1,2)= e_hahb*vr.XX*vphi.CC + e_ha2*vr.XX*vphi.CS - e_hb2*vr.XX*vphi.CS - e_hahb*vr.XX*vphi.SS;
sigma_yy(2,1) = sigma_yy(1,2);
sigma_yy = sigma_yy - y*y';
%Step4 calc Sigma_xy
% eq14
sigma_xy = zeros(3,2);
% 計算しやすいようにE[x*ha]等を先に計算しておく
e_xha = px*x_papp.XC - x_papp.XXC + py*x_papp.XS - x_papp.XYS;
e_xhb = py*x_papp.XC - x_papp.XYC - px*x_papp.XS + x_papp.XXS;
e_yha = px*x_papp.YC - x_papp.XYC + py*x_papp.YS - x_papp.YYS;
e_yhb = py*x_papp.YC - x_papp.YYC - px*x_papp.YS + x_papp.XYS;
e_thha = px*x_papp.ThC -x_papp.XThC + py*x_papp.ThS- x_papp.YThS;
e_thhb = py*x_papp.ThC -x_papp.YThC - px*x_papp.ThS+ x_papp.XThS;
sigma_xy(1,1) = e_xha*vr.X*vphi.C - e_xhb*vr.X*vphi.S; %x*y1
sigma_xy(1,2) = e_xhb*vr.X*vphi.C + e_xha*vr.X*vphi.S; %x*y2
sigma_xy(2,1) = e_yha*vr.X*vphi.C - e_yhb*vr.X*vphi.S; %y*y1
sigma_xy(2,2) = e_yhb*vr.X*vphi.C + e_yha*vr.X*vphi.S; %y*y2
sigma_xy(3,1) = e_thha*vr.X*vphi.C - e_thhb*vr.X*vphi.S; %theta*y1
sigma_xy(3,2) = e_thhb*vr.X*vphi.C + e_thha*vr.X*vphi.S; %theta*y2
sigma_xy = sigma_xy - x_p*y';
%calc gain K and x and sigma_x
% eq14
K = sigma_xy/(sigma_yy);
z = [r(i)*cos(phi(i));r(i)*sin(phi(i))];
x = x_p + K*(z-y);
sigma_x = sigma_p - K*sigma_yy*K';
result(i,:) = x';
end
結果比較
MKFによる自己推定の結果をオドメトリのみで推定した結果と，モーションキャプチャの結果（真値）と比較する．
オドメトリがどんどん真値から離れているのに対して，MKFを用いたほうはかなり真値と近い自己位置推定結果が出ており，
MKFの性能が示されておりちゃんと実装できていそう．
率直に強いフィルタだと思う．
コード
% 結果の表示
figure()
plot(result(:,1),result(:,2),'b-','LineWidth',1,'DisplayName','MKF')
hold on
plot(deadReconing(:,1),deadReconing(:,2),"r--",'LineWidth',1,'DisplayName','odometry')
plot(allData.trueX(1:steps),allData.trueY(1:steps),'k-.','LineWidth',1,'DisplayName','truth')
hold off
legend('FontSize',12,'Location','southwest')
xlabel('x [m]')
ylabel('y [m]')
ylim([-3.5,3.5])
axis('equal')
おわりに
今回，MKFの実装をMATLABにて行い，むしろ普通のカルマンフィルタへの理解が深まったと思う．
ほかのカルマンフィルタよりもノイズに強いらしく，面白いフィルタと思うが，
かなり手計算が大変であったので，ロボットの状態等が４変数以上になった場合に適用すると，地獄を見そう...
Discussion