超详细讲解无迹卡尔曼（UKF）滤波（个人整理结合代码分析）

2023-10-31

1.用来做什么?

2.线性卡尔曼滤波

3.扩展卡尔曼滤波

4.无迹卡尔曼滤波

1.用来做什么?

——针对系统的不确定性：1.不存在完美的数学模型

2.系统的扰动不可控、也很难建模

3.测量传感器存在误差

例1：通过系统的状态方程得出的电流值i1，和传感器测得的电流值i2，由于不确定性的存在，两个值都不准确，所以i1和i2通过卡尔曼滤波算法算出其最接近真实值的值。

例2：如小红同学说今天老师穿的是红色的衣服（根据以往经验，每周四老师都穿红衣服，小红得出的结论），小白说老师今天穿的白色的衣服（看到一个像老师的人穿的白色的衣服，小白得出老师穿白色衣服的结论），通过卡尔曼滤波推算谁说话的权重更大，则推断出更应该相信谁的话。

2.线性卡尔曼滤波

系统真实方程为（状态方程的基础上存在噪声）：

$x\:_{_{k}}=Ax_{_{k-1}}+B*U_{k-1}+w_{k-1}$

$Z_{k}=Hx_{k}+v_{k}$

先验估计（他叫这名）：

$\widehat{x}_{k}^{-}=A\widehat{x}_{k-1}+B*U_{k-1}$ —— 这是系统状态方程（方程建立的不准确性）

$P_{k}^{-}=AP_{k-1}A^{^{T}}+Q$ —— 这是先验误差协方差（用来算卡尔曼增益）

——（注意：噪声满足 $p(w)\rightarrow (0,Q)$ ,其中0是期望值， $Q$ 是协方差矩阵。 $P^{^{k-}}$ 这里是 $x_{k}-\widehat{x^{-}}_{k}$ 误差的期望/协方差（越接近0越好）。

$P_{k}^{-}=E[e^{-}_{k}*e^{-}_{k}{T}] =AE[e_{k-1}e_{k-1}^{T}]A^{T}+E[w_{k-1}w_{k-1}^{T}] =AP_{k-1}A^{T}+Q$

后验估计（所要求的值）：

因为： $Z_{k}=H\widetilde{x}_{k}$ ——（传感器测量的不准确性）

$\widetilde{x}_{k}=H^{-}Z_{k}$

$\widehat{x}_{k}=\widehat{x}_{k}^{-}+G(\widetilde{x}_{k}-\widehat{x}_{k}^{-})$ $G=K_{k}H$

$\widehat{x}_{k}=\widehat{x}_{k}^{-}+K_{k}(Z_{k}-H\widehat{x}_{k}^{-})$ ——后验得到的值

$P_{k}=E[e_{k}*e_{k}^{T}]$ —— $x_{k}-\widehat{x}_{k}$ （真实值与后验估计误差的期望(目标期望为0)）

$=E[(x_{k}-\widehat{x}_{k})(x_{k}-\widehat{x}_{k})^{T}]$

其目标为P的期望为0或方差最小（使后验估计与真实值误差最小），通过推导可以得到卡尔曼增益：

$K_{k}=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R}$ ——( $P$ 最小时求得的 $K_{k}$ ）

同时可以得到 $P$ ：

$P_{k}=(I-K_{k}H)P_{k}^{-}$ —— (用来更新 $P_{k}^{-}$ 值）

3.扩展卡尔曼滤波

卡尔曼滤波主要用于分析线性系统。

扩展卡尔曼滤波主要用于分析非线性系统。（正态分布的随机变量通过非线性系统后就不再是正态的了）。

扩展卡尔曼滤波——主要是通过泰勒级数一阶展开，将非线性系统线性化，之后的求取与线性化卡尔曼滤波一致。

非线性系统：

$x_{k}=f(x_{k-1},u_{k-1},w_{k-1})$ $P(w)\rightarrow N(0,Q)$

$Z_{k}=h(x_{k},v_{k})$ $P(v)\rightarrow N(0,R)$

泰勒级数一阶展开式：

$f(x)=f(x_{0})+\frac{\partial f}{\partial x}(x-x_{0})$ $\frac{\partial f}{\partial x}$ 多维用雅克比矩阵

对非线性系统线性化。系统有误差，无法在真实点线性化，则 ${\color{Red} {\color{Red} }f(x_{k})}$ 在 ${\color{Red} \widehat{x}_{k-1}}$ 处线性化（ ${\color{Green} \widehat{x}_{k-1}}$ 为 ${\color{Green} x-1}$ 时的后验估计）

$x_{k}=f(\widehat{x}_{k-1},u_{k-1},{\color{Red} w_{k-1}})+{\color{Red} A}(x_{k-1}-\widehat{x}_{k-1})+{\color{Red} W}w_{k-1}$

$w_{k-1}$ 等于0（误差假设为0） $A=\frac{\partial f}{\partial x}/\widehat{x}_{k-1},u_{k-1}$ 雅克比矩阵 $W=\frac{\partial f}{\partial w}/\widehat{x}_{k-1},u_{k-1}$ 雅克比矩阵。

$z_{k}$ 在 $\widehat{x}_{k}$ 线性化：

$z_{k}=h(\widehat{x}_{k},v_{k})+H(x_{k}-\widehat{x}_{k})+Vv_{k}$

$H=\frac{\partial h}{\partial x}/\widehat{x}_{k}$ , $V=\frac{\partial h}{\partial v}/\widehat{x}_{k}$ 。

令 $f(\widehat{x}_{k-1},u_{k-1},{0})=\widetilde{x}_{k-1}$ ， $h(\widehat{x}_{k},v_{k})=\widetilde{z}_{k}$ 。

$\rightarrow \left\{\begin{matrix}x_{k}=\widetilde{x}_{k-1}+{ A}(x_{k-1}-\widehat{x}_{k-1})+{ W}w_{k-1} \\ z_{k}=\widetilde{z}_{k}+H(x_{k}-\widehat{x}_{k})+Vv_{k} \end{matrix}\right.$ ——线性化后的系统方程

先验估计：

$\widehat{x}_{k}^{-}=f(\widehat{x}_{k-1},u_{k-1},{0})$

$P_{k}^{-}=AP_{k-1}A^{^{T}}+{\color{Red} WQW^{T}}$

后验估计：

$\widehat{x}_{k}=\widehat{x}_{k}^{-}+K_{k}(Z_{k}-{\color{Red} h(\widehat{x}_{k}^{-},0)})$

$K_{k}=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+{\color{Red} VRV^{T}}}$

$P_{k}=(I-K_{k}H)P_{k}^{-}$

标红部分为扩展和线性卡尔曼滤波的不同之处。

4.无迹卡尔曼滤波

由于扩展卡尔曼滤波可能存在线性化误差，且一般情况下雅克比矩阵不易实现，增加了算法的计算复杂度。

无迹卡尔曼滤波不采用泰勒展开实现非线性系统线性化，而是采用无迹变换（Unscented Transform，UT）来处理均值和协方差的非线性传递问题。（UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对雅克比矩阵进行求导。）

无迹变换：

——（1）原状态分布中按某一规则选取一些采样点（其均值和方差等于原状态分布的均值和方差）

—— （2）将点带入非线性方程中（求取变换后的均值和协方差）

以对称分布采样的UT变换为例。设一个非线性变换 $y=f(x)$ 。状态向量 $x$ 为 $n$ 维随机变量，已知其均值 $\overline{x}$ 和方差 $P$ 。通过UT变换得到2n+1个sigma点和相应的权值 $w$ 。

（1）计算2n+1个sigma点，即采样点

$\left\{\begin{matrix} X^{(0)}=\overline{{X}},i=0\\ X^{(i)}=\overline{{X}}+(\sqrt{(n+\lambda )P})_{i},i=1\sim n \\ X^{(i)}=\overline{{X}}-(\sqrt{(n+\lambda )P})_{i},i=1\sim n+1\sim 2n \end{matrix}\right.$

$(\sqrt{P})_{i}$ 表示矩阵方根的第 $i$ 列。注意应确保 $(n+\lambda )P$ 为半正定矩阵。

设:

$\overline{x}=\begin{bmatrix} 1\\ 2\\ 3\\ 4 \end{bmatrix}$ , $m=(\sqrt{(n+\lambda )P})_{i}=\begin{bmatrix} 1&1 &1 & 1\\ 0& 1& 1 &1 \\ 0 &0 &1 &1 \\ 0& 0& 0 &1 \end{bmatrix}$

则：

$\overline{x}+m=\begin{bmatrix} 2 &2 &2 &2 \\ 2 & 3&3 &3 \\ 3& 3 &4 & 4\\ 4 & 4 & 4 &5 \end{bmatrix}$ , $\overline{x}-m=\begin{bmatrix} 0 &0 &0 & 0\\ 2 & 1 & 1 & 1\\ 3 & 3 & 2 &2 \\ 4& 4 & 4 & 3 \end{bmatrix}$ —— $\overline{x}+m$ 与 $\overline{x}-m$ 对称

合并起来就是9个sigma点：

sig1 sig2 sig3 sig4 sig5 sig6 sig7 sig8 sig9

$X=\begin{bmatrix} 1&2 &2 &2 & 2& 0&0 & 0&0 \\ 2&2 &3 &3 &3 &2 &1 &1 &1 \\ 3& 3&3 &4 &4 & 3&3 & 2&2 \\ 4&4 & 4& 4& 5& 4&4 &4 &3 \end{bmatrix}$

（2）计算采样点相应的权值

$\left\{\begin{matrix} w_{m}^{(0)}=\frac{\lambda }{n+\lambda }\\ w_{c}^{(0)}=\frac{\lambda }{n+\lambda }+(1-a^{2}+\beta ) \\ w_{m}^{(i)}=w_{c}^{(i)}=\frac{\lambda }{2(n+\lambda) } ,i=1\sim 2n\end{matrix}\right.$

将9个sigma点带入非线性方程得到新的sigma点：

$\overline{X}=f(X)$

假设得到 sig1 sig2 sig3 sig4 sig5 sig6 sig7 sig8 sig9

$\overline{X}=\begin{bmatrix} 2&3 &3 &3 & 2& 1&1 &2&1 \\ 2&3 &2 &1 &1 &1 &2 &2 &2\\ 2&3&4 &3 &5 & 6&3 & 2&1 \\ 4&2&3& 2& 1& 3&1 &3 &1 \end{bmatrix}$ ，

定义权值 $w_{m}^{(1)}=-0.3$ ， $w_{m}^{(2)}=w_{m}^{(3)}=w_{m}^{(4)}=w_{m}^{(5)}=w_{m}^{(6)}=w_{m}^{(7)}=w_{m}^{(8)}=w_{m}^{(9)}=0.6$

可求得下一个点的先验值（经过UT变换后得到的先验值)

$\widehat{X}=\sum_{i=1}^{9}w_{m}^{i}X^{i}$

$\widehat{X}=-0.3*sigma1+0.6*sigma2+0.6*sigma3+0.6*sigma4+0.6*sigma5+0.6*sigma6+0.6*sigma7+0.6*sigma8+0.6*sigma9$

$\widehat{X}=\begin{bmatrix} 4\\ 5\\ 6\\ 7 \end{bmatrix}$ (随便填的值，我就不算了哈哈哈)

以上是无迹变换算先验值的整个过程。

无迹卡尔曼滤波算法：

非线性系统：

$\left\{\begin{matrix} X(k+1)=f(x(k),W(k))\\ Z(k)=h(x(k),V(k)) \end{matrix}\right.$

步骤：

————1、经过UT变换求得sigma采样点及其权值

$X^{(i)}(k/k)=\begin{bmatrix} \widehat{X}(k/k) & \widehat{X}(k/k)+\sqrt{(n+\lambda )P(k/k}) & \widehat{X}(k/k)-\sqrt{(n+\lambda )P(k/k}) \end{bmatrix}$

————2、计算2n+1个sigma点集的一步预测

$X^{^{(i)}}(k+1/k)=f[k,X^{i}(k/k)]$

————3、系统状态量的一步预测（相当于KF/EKF的先验值）

$\widehat{X}(k+1/k)=\sum_{i=0}^{2n}w^{i}X^{i}(k+1/k)$ ——UT变换后得到的新的状态值

$P(k+1/k)=\sum_{i=0}^{2n}w^{i}[\widehat{X}(k+1/k)-{X}^{i}(k+1/k)][\widehat{X}(k+1/k)-{X}^{i}(k+1/k)^{T}]+Q$

————4、再次使用UT变换，产生新的sigma点集

$\widehat{X}^{i}(k+1/k)=\begin{bmatrix} \widehat{X}(k+1/k) & \widehat{X}(k+1/k) +\sqrt{(n+\lambda )P(k+1/k}) &\widehat{X}(k+1/k) -\sqrt{(n+\lambda )P(k+1/k}) \end{bmatrix}$

————5、新的sigma点集带入观测方程，得到预测的观测量

$Z^{i}(k+1/k)=h[X^{i}(k+1/k)]$

————6、通过加权求得观测量新的均值及协方差

$\overline{Z}(k+1/k)=\sum_{i=0}^{2n}w^{i}Z^{i}(k+1/k)$ ——UT变换后得到的新的观测值

$Pz_{k}z_{k}=\sum_{i=0}^{2n}w^{i}[Z^{i}(k+1/k)]-\overline{Z}(k+1/k)][Z^{i}(k+1/k)]-\overline{Z}(k+1/k)]^{T}+R$

$Px_{k}z_{k}=\sum_{i=0}^{2n}w^{i}[X^{i}(k+1/k)]-\overline{X}(k+1/k)][Z^{i}(k+1/k)]-\overline{Z}(k+1/k)]^{T}$

————7、计算卡尔曼增益

$K(k+1)=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1}$

————8、系统的状态更新和协方差更新

$\widehat{X}(k+1/k+1)=\widehat{X}(k+1/k)+K(k+1)[Z(k+1)-\widehat{Z}(k+1/k)]$

$P(k+1/k+1)=P(k+1/k)-K(k+1)P_{z_{k}z_{k}}K^{T}(k+1)$

图1. 卡尔曼滤波流程图

实际应用（仿真分析）：

系统方程：

$X(k+1)=\Phi X(k)+ \Gamma W(k)$

$Z(k)=\sqrt{(x(k)-x_{0})^{2}+(y(k)-y_{0})^{2}}+V(k)$

$\Phi =\begin{bmatrix} 1& 1 & 0 & 0\\ 0& 1 & 0 &0 \\ 0& 0 & 1&1 \\ 0& 0& 0& 1 \end{bmatrix}$ , $\Gamma =\begin{bmatrix} 0.5 &0 \\ 1& 0\\ 0& 0.5\\ 0& 1 \end{bmatrix}$ , $Q=\sigma _{w}*diag([1,1])$ , $R=5$ 。

真实状态信息为：

$X_{real}(k)=[x_{real}(k),\dot{x}_{real}(k),y_{real}(k),\dot{y}_{real}(k)]^{T}$

UKF滤波算法得到的目标状态为：

$X_{UKF}(k)=[x_{UKF}(k),\dot{x}_{{UKF}}(k),y_{{UKF}}(k),\dot{y}_{{UKF}}(k)]^{T}$

定义均方根误差（RMSE）：

$RMSE(K)=\sqrt{(x_{UKF}(k)-x_{real}(k))^{2}+(y_{UKF}(k)-y_{real}(k))^{2}}$

代码：

T=1;
N=60/T;
X=zeros(4,N); ——定义X为4行60列的数列
X(:,1)=[-100,2,200,20]; ——第一个点
Z=zeros(1,N); ——定义Z为1行60列的数列
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;   ——可以随便给定（Z方程里的值）

Xstation=[x0,y0];
v=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)+v(t);
end ——真实状态值和观测值

L=4;
alpha=1;
kalpha=0;
belta=2;
ramda=3-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; ——初始协方差随便给定 (4行4列矩阵)
cho=(chol(P*(L+ramda)))'; ——半正定矩阵

for k=1:L
xgamaP1(:,k)=xestimate+cho(:,k);

xgamaP2(:,k)=xestimate-cho(:,k); —— xgamaP1的1列与xgamaP2的1列关于xestimate对称

xgamaP1的2列与xgamaP2的2列对称

xgamaP1的3列与xgamaP2的3列对称

xgamaP1的4列与xgamaP2的4列对称

end
Xsigma=[xestimate,xgamaP1,xgamaP2]; ——求出第一步的9个sigma点

Xsigmapre=F*Xsigma; ——9个sigma点带入非线性函数得到新的9个sigma点

Xpred=zeros(4,1);
for k=1:2*L+1
Xpred=Xpred+Wm(k)*Xsigmapre(:,k); ——新的状态值（先验值）4行1列

end

Ppred=zeros(4,4);
for k=1:2*L+1
Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';
end
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]; ——先验值经UT变化得到新的9个sigma点（用来算Z值）

for k=1:2*L+1;
Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation);——9个sigma点带入非线性函数得到新的9个 end sigma点，公式 $Z(k)=\sqrt{(x(k)-x_{0})^{2}+(y(k)-y_{0})^{2}}+V(k)$
Zpred=0;
for k=1:2*L+1
Zpred=Zpred+Wm(k)*Zsigmapre(1,k); ——新的Z值
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;

Pxz=zeros(4,1);
for k=1:2*L+1
Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)'; ——用来求卡尔曼增益
end

K=Pxz*inv(Pzz);

xestimate=Xpred+K*(Z(t)-Zpred); ——最终求得的值

P=Ppred-K*Pzz*K'; ——更新协方差值
P0=P;
Xukf(:,t)=xestimate; ——迭代
end

for i=1:N
Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));
end
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k.');
plot(Xukf(1,:),Xukf(3,:),'-r+');
legend('真实轨迹','UKF轨迹')
figure
hold on;box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r')

调用函数1：

function d=Dist(X1,X2)
if length(X2)<=2
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
else
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end

调用函数2：
function[y]=hfun(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);

参考视频：

【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili

参考书本：

卡尔曼滤波原理及应用——MATLAB仿真

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)