1.KF、EKF、UKF都是高斯濾波，下面的介紹一下他們的優缺點：
KF優點：計算簡單
KF缺點：高斯線性模型約束
EKF優點：可以近似非線性問題
EKF缺點：高斯噪聲約束，線性化引入了誤差會可能導致濾波發散，雅克比矩陣（一階）及海塞矩陣（二階）計算困難
UKF優點：模型無損失，計算精度高
UKF缺點：高斯噪聲約束
前面的KF和EKF都是都將問題轉化爲線性高斯模型，所以可以直接解出貝葉斯遞推公式中的解析形式，方便運算。但對於非線性問題，EKF除了計算量大，還有線性誤差的影響，所以這裏引入UKF。對於求解非線性模型的貝葉斯遞推公式的主要困難在於如何解析的求解一步預測狀態分佈的概率、（觀測方程得到的）似然函數分佈密度以及後驗條件概率的分佈，EKF利用泰勒分解將模型線性化，在利用高斯假設解決了概率計算困難的問題。但是線性誤差的引入降低了模型精度。那麼我們換個思路，對於非線性模型，直接用解析的方式來求解貝葉斯遞推公式比較困難主要很難解析的得到各個概率分佈的均值和方差，但UT變換（一種計算非線性隨機變量各階矩的近似方法）卻可以較好的解決這個問題，通過一定規律的採樣和權重，可以近似獲得均值和方差。而且由於不敏變換對統計矩的近似精度較高。
首先在介紹無跡kalman濾波算法之前，介紹一下UT變換
接下來要計算權值。對求期望來說，第一個sigma點的權值爲：
對求方差來說，第一個sigma點的權值爲如下，式子中的β也爲一個常數
對於剩下的2n個sigma點來說求期望的權值和求方差的權值都相同：
上標表m爲均值，c爲協方差，下標爲第幾個採樣點。參數
因此，一共有三個常數(α,β,k)需要我們來設定。根據其它參考資料，選擇參數的經驗爲：
其中a決定sigma點的散步程度，通常取一個小的正值，k通常取0，β用來描述x的分佈信息，β最優值爲2。
下面介紹無跡kalman濾波算法matlab實現步驟：
例子如下：
1.第一步，獲取sigma點集：
這個對應matlab程序
function UKF
clc;clear;
T=1;
N=60/T;
X=zeros(4,N);
X(:,1)=[-100,2,200,20];
Z=zeros(1,N);
delta_w=1e-3;
Q=delta_w*diag([0.5,1]) ;
G=[T^2/2,0;T,0;0,T^2/2;0,T];
R=5;
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
x0=200;
y0=300;
Xstation=[x0,y0];
w=sqrtm(R)*randn(1,N);
for t=2:N
X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);
end
for t=1:N
Z(t)=Dist(X(:,t),Xstation)+w(t);
end
L=4;
alpha=1;
kalpha=0;
belta=2;
ramda=alpha^2*(L+kalpha)-L;
for j=1:2*L+1
Wm(j)=1/(2*(L+ramda));
Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf=zeros(4,N);
Xukf(:,1)=X(:,1);
P0=eye(4);
for t=2:N
xestimate= Xukf(:,t-1);
P=P0;
cho=(chol(P*(L+ramda)))';
for k=1:L
xgamaP1(:,k)=xestimate+cho(:,k);
xgamaP2(:,k)=xestimate-cho(:,k);
end
Xsigma=[xestimate,xgamaP1,xgamaP2];
上面通過
xestimate= Xukf(:,t-1);
P=P0;
cho=(chol(P*(L+ramda)))';
for k=1:L
xgamaP1(:,k)=xestimate+cho(:,k);
xgamaP2(:,k)=xestimate-cho(:,k);
end
Xsigma=[xestimate,xgamaP1,xgamaP2];
即可獲得Xsigma對應上面公式了。這裏chol是matlab把矩陣解爲上三角和下三角兩個解。
2.利用上面的Xsigma,帶入模型中預測k時刻計算k+1時刻的狀態：
Xsigmapre=F*Xsigma;
其中， Xsigmapre=F*Xsigma;對應上面的公式。
3.通過權值對sigma點進行預測，計算均值與協方差，
Xpred=zeros(4,1);
for k=1:2*L+1
Xpred=Xpred+Wm(k)*Xsigmapre(:,k);
end
Ppred=zeros(4,4);
for k=1:2*L+1
Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';
end
其中Xpred=Xpred+Wm(k)*Xsigmapre(:,k)和 Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)'分別對應上面公式。
4.根據預測值再做一次UT變換，等到新的sigma點集：
Ppred=Ppred+G*Q*G';
chor=(chol((L+ramda)*Ppred))';
for k=1:L
XaugsigmaP1(:,k)=Xpred+chor(:,k);
XaugsigmaP2(:,k)=Xpred-chor(:,k);
end
Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
5.計算觀測均值和方差
for k=1:2*L+1
Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation);
end
Zpred=0;
for k=1:2*L+1
Zpred=Zpred+Wm(k)*Zsigmapre(1,k);
end
Pzz=0;
for k=1:2*L+1
Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';
end
Pzz=Pzz+R;
6.計算Kalman增益：
K=Pxz*inv(Pzz);
7.狀態和方差更新：
xestimate=Xpred+K*(Z(t)-Zpred);
P=Ppred-K*Pzz*K';
P0=P;
Xukf(:,t)=xestimate;
最後，給出仿真結果：
完整代碼下載地址：
https://download.csdn.net/download/caokaifa/10718818