无迹卡尔曼滤波算法的实现

之前写了线性卡尔曼滤波以及拓展卡尔曼滤波算法的实现，今天补充一下无迹卡尔曼滤波算法的MATLAB算法实现。其实网上相关的博客很多，也讲的非常详细，所以本博客不再对原理部分进行过多的赘述。本篇将围绕运动模型的介绍、算法的原理及MATLAB代码实现展开。

卡尔曼滤波算法的核心

不管是线性、拓展或者无迹卡尔曼滤波算法，算法都应该具备两个基本模型：信号模型、观测模型。在自动驾驶中所谓的信号模型也就是目标的运动模型，观测模型就是我们观测的方式，比如说激光、雷达、GPS等。

1. 运动模型

下面给出一下基本的运动模型以及模型之间的关系示意图

一般的讨论还是基于CV模型和CTRV模型，从上述右图可以看出，在转弯率为0时，两种模型等效。

关于一次模型与二次模型是如何定义的？

一次运动模型：轨迹是线性
二次运动模型：轨迹是曲线
当然也可以认为一维、二维的区别

匀速运动模型

我们可以利用经典力学关系式来得到：
x = ( p x , p y , v x , v y ) T p x ′ = p x + v x Δ t + a x Δ t 2 2 p y ′ = p y + v y Δ t + a y Δ t 2 2 v x ′ = v x + a x Δ t v y ′ = v y + a y Δ t \begin{aligned} & {x} = ({p_x},{p_y},{v_x},{v_y})^T \\ & p{'_x} = {p_x} + {v_x}\Delta t + \frac{{{a_x}\Delta {t^2}}}{2} \\ & p{'_y} = {p_y} + {v_y}\Delta t + \frac{{{a_y}\Delta {t^2}}}{2} \\ & v{'_x} = {v_x} + {a_x}\Delta t \\ & v{'_y} = {v_y} + {a_y}\Delta t \\ \end{aligned} x=(px,py,vx,vy)Tpx′=px+vxΔt+2axΔt2py′=py+vyΔt+2ayΔt2vx′=vx+axΔtvy′=vy+ayΔt 然后可以建立我们的状态方程，也就是信号模型如下：
( p x ′ p y ′ v x ′ v y ′ ) = ( 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ) ( p x p y v x v y ) + ( Δ t 2 2 Δ t 2 2 Δ t Δ t ) ( a x a y a x a y ) \left( \begin{array}{l}p{'_x}\\p{'_y}\\v{'_x}\\v{'_y}\end{array} \right) = \left( {\begin{array}{ccccccccccccccc}1&0&{\Delta t}&0\\0&1&0&{\Delta t}\\0&0&1&0\\0&0&0&1\end{array}} \right)\left( \begin{array}{l}{p_x}\\{p_y}\\{v_x}\\{v_y}\end{array} \right) + \left( \begin{array}{l}\frac{{\Delta {t^2}}}{2}\\\frac{{\Delta {t^2}}}{2}\\\Delta t\\\Delta t\end{array} \right)\left( \begin{array}{l}{a_x}\\{a_y}\\{a_x}\\{a_y}\end{array} \right) ⎝⎜⎜⎛px′py′vx′vy′⎠⎟⎟⎞=⎝⎜⎜⎛10000100Δt0100Δt01⎠⎟⎟⎞⎝⎜⎜⎛pxpyvxvy⎠⎟⎟⎞+⎝⎜⎜⎛2Δt22Δt2ΔtΔt⎠⎟⎟⎞⎝⎜⎜⎛axayaxay⎠⎟⎟⎞ 这其实是一个带有控制项的线性卡尔曼滤波的状态方程。但是我们的匀速模型中没有加速度哇，这时候我们就可以把加速度作为我们状态方程的扰动误差。这是时候我们就可以将加速度建模为一个高斯分布的噪声，然后可以得到最终的状态方程表达式。这就是上一篇中KF与EKF算法的运动模型假设。

恒定转弯率和速度幅度模型

因为涉及转弯，所以这个模型的状态矢量中需要增加角度、角速度两个参量，同时速度也变为了径向速度。此时的运动模型求解需要利用积分公式进行，可以使用一个计算积分的工具WolframAlpha

这里直接给出计算后的结果：
x k = [ p x p y v ψ ψ ˙ ] > > x k + 1 = x k + [ v k ψ ˙ k ( sin ⁡ ( ψ k + ψ ˙ k Δ t ) − sin ⁡ ( ψ k ) ) v k ψ ˙ k ( − cos ⁡ ( ψ k + ψ ˙ k Δ t ) + cos ⁡ ( ψ k ) ) 0 ψ ˙ k Δ t 0 ] + [ 1 2 v a , k cos ⁡ ( ψ k ) Δ t 2 1 2 v a , k sin ⁡ ( ψ k ) Δ t 2 v a , k Δ t 1 2 v ψ ¨ , k Δ t 2 v ψ ¨ , k Δ t ] {\bf{x_k}} = \left[ {\begin{array}{ccccccccccccccc}{{p_x}}\\{{p_y}}\\v\\\psi \\{\dot \psi }\end{array}} \right] >> {{\bf{x}}_{{\bf{k + 1}}}} = {{\bf{x}}_{\bf{k}}} + \left[ {\begin{array}{ccccccccccccccc}{\frac{{{v_k}}}{{{{\dot \psi }_k}}}(\sin ({\psi _k} + {{\dot \psi }_k}\Delta t) - \sin ({\psi _k}))}\\{\frac{{{v_k}}}{{{{\dot \psi }_k}}}( - \cos ({\psi _k} + {{\dot \psi }_k}\Delta t) + \cos ({\psi _k}))}\\0\\{{{\dot \psi }_k}\Delta t}\\0\end{array}} \right] + \left[ {\begin{array}{ccccccccccccccc}{\frac{1}{2}{v_{a,k}}\cos ({\psi _k})\Delta {t^2}}\\{\frac{1}{2}{v_{a,k}}\sin ({\psi _k})\Delta {t^2}}\\{{v_{a,k}}\Delta t}\\{\frac{1}{2}{v_{\ddot \psi ,k}}\Delta {t^2}}\\{{v_{\ddot \psi ,k}}\Delta t}\end{array}} \right] xk=⎣⎢⎢⎢⎢⎡pxpyvψψ˙⎦⎥⎥⎥⎥⎤>>xk+1=xk+⎣⎢⎢⎢⎢⎢⎡ψ˙kvk(sin(ψk+ψ˙kΔt)−sin(ψk))ψ˙kvk(−cos(ψk+ψ˙kΔt)+cos(ψk))0ψ˙kΔt0⎦⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎡21va,kcos(ψk)Δt221va,ksin(ψk)Δt2va,kΔt21vψ¨,kΔt2vψ¨,kΔt⎦⎥⎥⎥⎥⎤

在运动模型中，因为速度是可以观测的，所以一般扰动误差都可以认为是来源于加速度，所以对于CTRV模型，我们将径向加速度以及角加速度建模为扰动噪声。从上式的公式可以看出，角速度是不可以为0的，因为它在分母上，也就意味着不能是CV模型。而对于CV模型来说，这种含有5个变量的状态方程可以写为 x k + 1 = x k + [ v k cos ⁡ ( ψ k ) Δ t v k sin ⁡ ( ψ k ) Δ t 0 0 0 ] + [ 1 2 v a , k cos ⁡ ( ψ k ) Δ t 2 1 2 v a , k sin ⁡ ( ψ k ) Δ t 2 v a , k Δ t 1 2 v ψ ¨ , k Δ t 2 v ψ ¨ , k Δ t ] {{\bf{x}}_{{\bf{k + 1}}}} = {{\bf{x}}_{\bf{k}}} + \left[ {\begin{array}{ccccccccccccccc}{{v_k}\cos ({\psi _k})\Delta t}\\{{v_k}\sin ({\psi _k})\Delta t}\\0\\0\\0\end{array}} \right]+ \left[ {\begin{array}{ccccccccccccccc}{\frac{1}{2}{v_{a,k}}\cos ({\psi _k})\Delta {t^2}}\\{\frac{1}{2}{v_{a,k}}\sin ({\psi _k})\Delta {t^2}}\\{{v_{a,k}}\Delta t}\\{\frac{1}{2}{v_{\ddot \psi ,k}}\Delta {t^2}}\\{{v_{\ddot \psi ,k}}\Delta t}\end{array}} \right] xk+1=xk+⎣⎢⎢⎢⎢⎡vkcos(ψk)Δtvksin(ψk)Δt000⎦⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎡21va,kcos(ψk)Δt221va,ksin(ψk)Δt2va,kΔt21vψ¨,kΔt2vψ¨,kΔt⎦⎥⎥⎥⎥⎤需要补充的一点是，由于我们的扰动噪声也具有非线性效应，所以需要对状态矢量进行增广如下：
x = [ p x p y v ψ ψ ˙ ] ⇒ x a = [ p x p y v ψ ψ ˙ v a v ψ ¨ ] , P a = [ P 0 0 Q ] , Q = [ σ v a 2 0 0 σ v ψ ¨ 2 ] {\bf{x}} = \left[ {\begin{array}{ccccccccccccccc}{{p_x}}\\{{p_y}}\\v\\\psi \\{\dot \psi }\end{array}} \right] \Rightarrow {{\bf{x}}_{\bf{a}}} = \left[ {\begin{array}{ccccccccccccccc}{{p_x}}\\{{p_y}}\\v\\\psi \\{\dot \psi }\\{{v_a}}\\{{v_{\ddot \psi }}}\end{array}} \right],{{\bf{P}}_{\bf{a}}}{\bf{ = }}\left[ {\begin{array}{ccccccccccccccc}{\bf{P}}&{\bf{0}}\\{\bf{0}}&{\bf{Q}}\end{array}} \right],{\bf{Q}} = \left[ {\begin{array}{ccccccccccccccc}{\sigma _{{v_a}}^{\rm{2}}}&0\\0&{\sigma _{{v_{\ddot \psi }}}^{\rm{2}}}\end{array}} \right] x=⎣⎢⎢⎢⎢⎡pxpyvψψ˙⎦⎥⎥⎥⎥⎤⇒xa=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡pxpyvψψ˙vavψ¨⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤,Pa=[P00Q],Q=[σva200σvψ¨2] 至于具体为什么要增广，我理解的也不是很到位，反正对于无迹卡尔曼增广只用在了协方差矩阵分解、状态方程建立上。观测模型中还是一个5维的向量。信号模型建立完之后，我们就可以根据当前的状态值来预测我们下一时刻的值。

2. 观测模型

还是基于激光和雷达数据的观测，这里就不再过多赘述了，过一下拓展卡尔曼滤波和无迹卡尔曼滤波的区别

当高斯分布经过非线性变换之后可能不再是高斯分布了，可以得到一个最优近似的高斯分布。EKF就是通过将非线性变换线性化来时逼近最优解，但是有时候估计的结果偏差较大；而无迹卡尔曼则是利用对多个点（Sigma点集）的非线性变换结果直接拟合出最优的高斯分布。

Sigma点集的构建
χ [ 0 ] = x χ [ i ] = x + ( ( n + λ ) Σ ) i i = 1 , ⋯ , n χ [ i ] = x − ( ( n + λ ) Σ ) i − n i = n + 1 , ⋯ , 2 n \begin{array}{ccc}{{\bf{\chi }}^{[0]}} = x \\\begin{array}{l}{{{\bf{\chi }}^{[i]}} = x + {{\left( {\sqrt {(n + \lambda )\Sigma } } \right)}_i}}&{i = 1, \cdots ,n}\end{array}\\\begin{array}{l}{{{\bf{\chi }}^{[i]}} = x - {{\left( {\sqrt {(n + \lambda )\Sigma } } \right)}_{i - n}}}&{i = n + 1, \cdots ,2n}\end{array}\end{array} χ[0]=xχ[i]=x+((n+λ)Σ )ii=1,⋯,nχ[i]=x−((n+λ)Σ )i−ni=n+1,⋯,2n通过上式可以得到Sigma点集， Σ \Sigma Σ代表的是协方差。这里的矩阵求根操作，在代码中使用的是Cholesky 分解。
Sigma点集预测
通过状态方程对点集中的所有点进行预测，然后对结果进行加权得到最终的结果，加权因子计算公式如下：
ω m [ 0 ] = λ n + λ ω c [ 0 ] = ω m [ 0 ] + ( 1 − α 2 + β ) ω m [ i ] = ω c [ i ] = 1 2 ( n + λ ) i = 1 , ⋯ , 2 n \begin{array}{c}{\bf{\omega }}_m^{[0]} = \frac{\lambda }{{n + \lambda }}\\{\bf{\omega }}_c^{[0]} = {\bf{\omega }}_m^{[0]} + (1 - {\alpha ^2} + \beta )\\\begin{array}{ccccccccccccccc}{{\bf{\omega }}_m^{[i]} = {\bf{\omega }}_c^{[i]} = \frac{1}{{2(n + \lambda )}}}&{i = 1, \cdots ,2n}\end{array}\end{array} ωm[0]=n+λλωc[0]=ωm[0]+(1−α2+β)ωm[i]=ωc[i]=2(n+λ)1i=1,⋯,2n利用加权因子可以得到预测后的结果表达式如下：
x ′ = ∑ i = 0 2 n ω [ i ] g ( χ [ i ] ) Σ ′ = ∑ i = 0 2 n ω [ i ] ( g ( χ [ i ] ) − x ′ ) ( g ( χ x [ i ] ) − x ′ ) T + R \begin{array}{c}{\bf{x '}} = \sum\limits_{i = 0}^{2n} {{{\bf{\omega }}^{[i]}}g({{\bf{\chi }}^{[i]}})} \\{\bf{\Sigma '}} = \sum\limits_{i = 0}^{2n} {{{\bf{\omega }}^{[i]}}(g({{\bf{\chi }}^{[i]}}) - {\bf{x '}}){{(g({{\bf{\chi }}^{x[i]}}) - {\bf{x '}})}^T}} + {\bf{R}}\end{array} x′=i=0∑2nω[i]g(χ[i])Σ′=i=0∑2nω[i](g(χ[i])−x′)(g(χx[i])−x′)T+R
Sigma点观测
通过观测模型对所有预测值进行观测，利用加权因子得到最终的观测结果：
Z = h ( χ ) z ^ = ∑ i = 0 2 n ω [ i ] Z [ i ] \begin{array}{l}{\bf{Z}} = h({\bf{\chi }})\\{\bf{\hat z}} = \sum\limits_{i = 0}^{2n} {{{\bf{\omega }}^{[i]}}{{\bf{Z}}^{[i]}}} \end{array} Z=h(χ)z^=i=0∑2nω[i]Z[i]
更新
计算卡尔曼增益：
T = ∑ i = 0 2 n ω [ i ] ( χ [ i ] − μ ′ ) ( Z [ i ] − z ^ ) T ⏟ P ′ H j T S = ∑ i = 0 2 n ω [ i ] ( Z [ i ] − z ^ ) ( Z [ i ] − z ^ ) T + Q K = T ∙ S − 1 \begin{array}{l}{\bf{T = }}\sum\limits_{i = 0}^{2n} {\underbrace {{{\bf{\omega }}^{[i]}}{\bf{(}}{{\bf{\chi }}^{[i]}}{\bf{ - \mu ')(}}{{\bf{Z}}^{[i]}}{\bf{ - \hat z}}{{\bf{)}}^{\bf{T}}}}_{{\bf{P'H}}_{\bf{j}}^{\bf{T}}}} \\{\bf{S = }}\sum\limits_{i = 0}^{2n} {{{\bf{\omega }}^{[i]}}{\bf{(}}{{\bf{Z}}^{[i]}}{\bf{ - }}\hat z{\bf{)(}}{{\bf{Z}}^{[i]}}{\bf{ - }}\hat z{{\bf{)}}^{\bf{T}}}} {\bf{ + Q}}\end{array} \\{\bf{K = T \bullet }}{{\bf{S}}^{{\bf{ - 1}}}} T=i=0∑2nP′HjT ω[i](χ[i]−μ′)(Z[i]−z^)TS=i=0∑2nω[i](Z[i]−z^)(Z[i]−z^)T+QK=T∙S−1
更新均值和方差：
μ = μ ′ + K ( z − z ^ ) Σ = ( I − K T ) Σ ′ \begin{array}{l}{\bf{\mu = \mu ' + K(z - }}\hat z{\bf{)}}\\{\bf{\Sigma = (I - KT)\Sigma '}}\end{array} μ=μ′+K(z−z^)Σ=(I−KT)Σ′到此无迹卡尔曼滤波算法就结束了，还是很经典的。

3.实验结果

实验代码

clear
close all
data0=importdata('data.txt');
aa=[4.632272e-01 6.074152e-01  1477010443000000 6.000000e-01 6.000000e-01  2.199937e+00    0    0   6.911322e-03  0];
data=[aa;data0.data];
[m,n]=size(data);
Z=data(2:2:m-1,1:3)';%%观测数据
X=zeros(4,249);%%观测数据直角坐标系结果
X(1,:) = Z(1,:) .* cos(Z(2,:));
X(2,:) = Z(1,:) .* sin(Z(2,:));
X(3,:) = Z(3,:) .* cos(Z(2,:));
X(4,:) = Z(3,:) .* sin(Z(2,:));
Zhenshiguiji=data(2:2:m-1,5:6)';%%真实数据
[mm,nn]=size(Z);
%% 初始化
delta_t=100000/1000000; %间隔0.1s
%%需要用到的方差
std_yawdd=0.7;
std_a_=2;
std_radr=0.3;
std_radphi=0.03;
std_radrd=0.3;
R=[std_radr*std_radr,0,0;0,std_radphi*std_radphi,0;0,0,std_radrd*std_radrd];
P_=[1, 0, 0, 0, 0;0, 1, 0, 0, 0;0, 0, 1, 0, 0;0, 0, 0, 1, 0;0, 0, 0, 0, 1;];
%%sigma点需要用到的参量
n_x_=5; % 状态矢量维度
n_aug_=7; % 用到的sigma点
lambda=3 - n_aug_;%%取了-4
weights=[lambda/((lambda+n_aug_)),(1/(2*(lambda+n_aug_)))*ones(1,2*n_aug_)];%
Xukf=zeros(5,249);
Xukf(1,1)=X(1,1);
Xukf(2,1)=X(2,1);
%%%%%%%%%%%%%%%%%%%%
for k=2:249
%%预测过程
%1.sigma点生成
%增强状态矩阵X_aug=zeros(7,1);X_aug=[Xukf(:,k-1);0;0];P_aug=zeros(7,7);P_aug(1:5,1:5)=P_;P_aug(6,6)=std_a_*std_a_;P_aug(7,7)=std_yawdd*std_yawdd;
%     P_aug=nearestPSD(P_aug);cho=(chol(P_aug*(n_aug_+lambda)))';
%     cho=(sqrtm(P_aug*(n_aug_+lambda)))';for i=1:n_aug_xgamaP1(:,i)=X_aug+cho(:,i);xgamaP2(:,i)=X_aug-cho(:,i);endX_sig=[X_aug,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点
%2.sigma点集预测for i =1: 2*n_aug_+1input_x = X_sig(:,i);px = input_x(1);py = input_x(2);v = input_x(3);psi = input_x(4);psi_dot = input_x(5);mu_a = input_x(6);mu_psi_dot_dot = input_x(7);if(psi_dot < 0.001)term2(:,i) = [v * cos(psi) * delta_t;v * sin(psi) * delta_t;0; psi_dot * delta_t;0];term3(:,i) = [0.5 * delta_t*delta_t * cos(psi) * mu_a;0.5 * delta_t*delta_t * sin(psi) * mu_a;delta_t * mu_a;0.5 * delta_t*delta_t * mu_psi_dot_dot;delta_t * mu_psi_dot_dot];result = X_sig(1:5,i) + term2(:,i) + term3(:,i);elseterm2(:,i) = [(v/psi_dot) * (sin(psi + psi_dot * delta_t) - sin(psi));(v/psi_dot) * (-cos(psi + psi_dot * delta_t) + cos(psi));0;psi_dot * delta_t;0];term3(:,i)= [0.5 * delta_t*delta_t * cos(psi) * mu_a;0.5 * delta_t*delta_t * sin(psi) * mu_a;delta_t * mu_a;0.5 * delta_t*delta_t * mu_psi_dot_dot;delta_t * mu_psi_dot_dot];result = X_sig(1:5,i) + term2(:,i) + term3(:,i);end Xsig_pred(:,i) = result; % 5*1end%得到最终预测值P_=zeros(5,5);for i=1:2*n_aug_+1Xukf(:,k)=Xukf(:,k)+(weights(i)*Xsig_pred(:,i));%%endfor i=1:2*n_aug_+1x_diff = Xsig_pred(:,i) - Xukf(:,k);while x_diff(4)> pix_diff(4) = x_diff(4)-2.*pi;endwhile x_diff(4) <-pix_diff(4)=x_diff(4)+2.*pi;endP_ = P_ + weights(i) * (x_diff * x_diff.');end%3.更新%将所有点通过观测模型，这里不需要写出观测矩阵Zsig=zeros(3,2*n_aug_+1);for i=1:2*n_aug_+1%-----------状态方程与观测的一个转换-----------px = Xsig_pred(1,i);py = Xsig_pred(2,i);v = Xsig_pred(3,i);psi = Xsig_pred(4,i);psi_dot = Xsig_pred(5,i);%是一个5维对应3维的过程temp = px * px + py * py;rho = sqrt(temp);phi = atan2(py, px);if(abs(rho) < 0.0001)rho_dot = 0;elserho_dot =(px * cos(psi) * v + py * sin(psi) * v)/rho;%------------------endtemp1 = [rho; phi; rho_dot];Zsig(:,i) = temp1;endz_pred=zeros(3,1);for i=1:2*n_aug_+1z_pred = z_pred + weights(i) * Zsig(:,i);end%所有sigma的滤波结果进行加权得到最终的结果%求SS=zeros(3,3);for i=1:2*n_aug_+1z_diff = Zsig(:,i) - z_pred;while z_diff(2)> piz_diff(2) = z_diff(2)-2.*pi;endwhile z_diff(2) <-piz_diff(2)=z_diff(2)+2.*pi;endS = S + weights(i) * (Zsig(:,i) - z_pred) * (Zsig(:,i) - z_pred)';endS = S + R;%求TT=zeros(n_x_,3);for i=1:2*n_aug_+1x_diff = Xsig_pred(:,i) - Xukf(:,k);while x_diff(4)> pix_diff(4) = x_diff(4)-2.*pi;endwhile x_diff(4) <-pix_diff(4)=x_diff(4)+2.*pi;endz_diff = Zsig(:,i) - z_pred;while z_diff(2)> piz_diff(2) = z_diff(2)-2.*pi;endwhile z_diff(2) <-piz_diff(2)=z_diff(2)+2.*pi;endT = T + weights(i) * x_diff * z_diff';end%求卡尔曼增益K(:,:,k-1)=T/S;y=Z(:,k)-z_pred;while (y(2)> pi) y(2)=y(2)-2*pi;endwhile (y(2)<-pi) y(2)=y(2)+2*pi;endXukf(:,k)= Xukf(:,k)+K(:,:,k-1)*y;P_=P_-K(:,:,k-1)*S*K(:,:,k-1)';
endfigure
plot(Xukf(1,:), Xukf(2,:),'b-','Linewidth', 2)
hold on
scatter(Z(1,:) .* cos(Z(2,:)), Z(1,:) .* sin(Z(2,:)),7,'MarkerEdgeColor',[0 .5 .5],...'MarkerFaceColor',[0 .7 .7])
hold on
scatter(Zhenshiguiji(1,:), Zhenshiguiji(2,:),7,'MarkerEdgeColor',[0.5 0 0],...'MarkerFaceColor',[0.7 0 0])
legend("估计轨迹","观测轨迹","真实轨迹")
grid on
figure
V_zhenshi=data(2:2:m-1,7:8)';
plot(V_zhenshi(1,:),'b-')
hold on
evx=smooth(Xukf(3,:).*cos(Xukf(4,:)),9);
plot(evx,'r-')
grid on
legend('X方向真实速度','X方向估计速度')
figure
V_zhenshi=data(2:2:m-1,7:8)';
plot(V_zhenshi(2,:),'b-')
hold on
evy=smooth(Xukf(3,:).*sin(Xukf(4,:)));
plot(evy,'r-')
grid on
legend('Y方向真实速度','Y方向估计速度')
%%计算估计RMSE
%对四个值计算RMSE
%计算公式为真值减估计值加平方后累加除以数据长度再开平方
rmse_x=sqrt(sum((Xukf(1,:)-Zhenshiguiji(1,:)).^2)/length(Z(1,:)));
rmse_y=sqrt(sum((Xukf(2,:)-Zhenshiguiji(2,:)).^2)/length(Z(2,:)));
rmse_vx=sqrt(sum((evx'-V_zhenshi(1,:)).^2)/length(Z(2,:)));
rmse_vy=sqrt(sum((evy'-V_zhenshi(2,:)).^2)/length(Z(2,:)));disp('估计性能分析:')
disp(['距离估计RMSE：X坐标=',num2str(rmse_x)])
disp(['距离估计RMSE：Y坐标=',num2str(rmse_y)])
disp(['速度估计RMSE：V_x=',num2str(rmse_vx)])
disp(['速度估计RMSE：V_y=',num2str(rmse_vy)])

代码编写过程中遇到了乱初始化变量导致P_aug矩阵非正定而无法分解的问题，检查了很久才解决这个问题。
实验结果
在距离估计中结果是优于EKF的，代码中给出了均方误差的计算。在速度估计中略优于EKF。

#----------------------------------------------------------------------------参考----------------------------------------------------------------------------------#
https://www.it610.com/article/1291765801656852480.htm
https://zhuanlan.zhihu.com/p/141059329

【自动驾驶中的卡尔曼滤波算法——无迹卡尔曼滤波的实现】相关推荐

自动驾驶中的机器学习
出品:CSDN(ID:CSDNnews) 作者:Denis Chikurtev 译者:马超责编:晋兆雨以下为译文: 近年来,自动驾驶技术技术的发展速度非常快.预计达到完全自动驾驶L5的程度 ...
【自动驾驶】20.自动驾驶中的各种时空坐标系
简介介绍自动驾驶技术中几种常用的坐标系统,以及他们之间如何完成关联和转换,最终构建出统一的环境模型. 所谓时空坐标系,包括三维空间坐标系和一维时间坐标系.在此基础上,用解析的形式(坐标)把物体在空间 ...
自动驾驶中的SLAM
二十余年来,同时定位与建图(Simultaneous Localization And Mapping,SLAM)在移动机器人技术社区中一直是一个非常受欢迎的话题.SLAM有很多应用,例如空间探索和自 ...
手撕自动驾驶算法——无迹卡尔曼滤波（UKF）
文章目录 1. 简介 2. CTRV运动模型 2.1 CTRV的目标状态量: 2.2 CTRV的状态转移函数: 2.3 CTRV Process Noise 3. Prediction 3.1 Gen ...
滤波算法 | 无迹卡尔曼滤波（UKF）算法及其MATLAB实现
目录简介 UKF滤波滤波流程和公式 MATLAB程序结论简介本文接着分享位姿跟踪和滤波算法中用到的一些常用程序,希望为后来者减少一些基础性内容的工作时间.以往分享总结见文章:位姿跟踪 | 相 ...
自动驾驶中的9种传感器融合算法
来源丨AI 修炼之路在自动驾驶汽车中,传感器融合是融合来自多个传感器数据的过程.该步骤在机器人技术中是强制性的,因为它提供了更高的可靠性.冗余性以及最终的安全性. 为了更好地理解,让我们考虑一个简单 ...
自动驾驶中常用的四类机器学习算法
来源:智车科技机器学习算法已经被广泛应用于自动驾驶各种解决方案,电控单元中的传感器数据处理大大提高了机器学习的利用率,也有一些潜在的应用,比如利用不同外部和内部的传感器的数据融合(如激光雷达.雷达. ...
一文看懂自动驾驶中应用的机器学习算法
本文来自AI新媒体量子位(QbitAI) 机器学习算法已经被广泛应用于自动驾驶各种解决方案,电控单元中的传感器数据处理大大提高了机器学习的利用率,也有一些潜在的应用,比如利用不同外部和内部的传感器的数 ...
人工智能算法在自动驾驶中的应用
前几天一个新闻挺搞笑的,美国一辆"自动驾驶"状态的特斯拉被交警拦停,驾驶员在车里竟然睡着了--甚至还狡辩自己没有驾车因此不涉嫌酒驾.龙叔忽然想起来,这类事件已经不是第一次发生了,于 ...

【自动驾驶中的卡尔曼滤波算法——无迹卡尔曼滤波的实现】