文章目录

1 前言2 通过无迹变换实现线性化3 无迹卡尔曼滤波算法4 实例5 总结6 参考文献

1 前言

前文介绍了扩展卡尔曼滤波，它对非线性函数线性化的方法是泰勒展开式，但这不是唯一线性化的方法本文介绍的无迹卡尔曼滤波(unscented Kalman filter, UKF)是应用无迹变换进行线性化无迹变换要比泰勒展开式更加准确，所以对于非线性滤波效果更好

2 通过无迹变换实现线性化

这里不做公式推导和证明，仅给出算法所需无迹变换实现线性化的公式无迹变换是通过从高斯分布中提取$\sigma$点(${\chi }^{[i]}$)，再对这些$\sigma$点进行非线性变换，具体方法见第3部分对于$n$维高斯分布要提取$2n+1$个$\sigma$点(${\chi }^{[i]}$)这些$\sigma$点(${\chi }^{[i]}$)位于均值附近，对称分布: $$\begin{gathered} \begin{array}{rl}{\chi }^{[0]}& =\mu \\ {\chi }^{[i]}& =\mu +{\left(\sqrt{(n+\lambda )\Sigma }\right)}_{i}i=1,\cdots ,n\\ {\chi }^{[i]}& =\mu -{\left(\sqrt{(n+\lambda )\Sigma }\right)}_{i-n}i=n+1,\cdots ,2n\end{array} \\ \text{式中：}\lambda ={\alpha }^2(n+k)-n(\alpha ,k\text{:为尺度参数，决定σ点距离均值的远近}) \\ \text{注意：(n+λ)Σ 是矩阵开平方，例如：B =A⇒ATA=B, matlab函数为sqrtm()} \end{gathered}$$有两个权重： $w_m^{[i]}$:用于计算均值$w_c^{[i]}$:用于计算方差 $$\begin{array}{rl}{w}_m^{[0]}& =\frac{\lambda }{n+\lambda }\\ w_c^{[0]}& =\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )\\ w_m^{[i]}& =w_c^{[i]}=\frac{1}{2(n+\lambda )}i=1,\cdots ,2n\end{array}$$ 对$\sigma$点(${\chi }^{[i]}$)进行非线性变换： $$y^{[i]}=g({\chi }^{[i]})$$从变换后的点$y^{[i]}$中计算均值和方差 $$\begin{array}{rl}{\mu }^{\prime }& =\sum _{i=0}^{2n}{w}_m^{[i]}{y}^{[i]}\\ {\Sigma }^{\prime }& =\sum _{i=0}^{2n}{w}_c^{[i]}(y^{[i]}-{\mu }^{\prime })(y^{[i]}-{\mu }^{\prime }{)}^T\end{array}$$到这里无迹变换线性化的公式就结束了，我们看看无迹卡尔曼滤波算法是怎样的步骤

3 无迹卡尔曼滤波算法

这里给出无迹卡尔曼滤波算法，并与扩展卡尔曼滤波做一下简要对比：

第$2\sim 5$行：是扩展卡尔曼滤波的第$3,4$行，进行状态预测 第$2$行：根据先验置信度计算$\sigma$点(${\chi }_{t-1}$)第$3$行：根据$\sigma$点(${\chi }_{t-1}$)运用非线性状态预测函数$g$进行状态预测第$4,5$行：根据预测的状态计算该状态的均值和方差(具体公式不做解释) 第$6\sim 13$行：是扩展卡尔曼滤波的第$4\sim 6$行，进行测量更新 第$6$行：计算新的$\sigma$点($\overline{{\chi }_t}$)第$7$行：对新的$\sigma$点($\overline{{\chi }_t}$)进行观测值预测第$8,9$行：计算观测值${\hat{z}}_t$和不确定度$S_t$，对应于卡尔曼滤波即是计算$H_t{\bar{\Sigma }}_t{H}_t^T+Q_t$第$10$行：计算状态和观测值的协方差矩阵,对应于卡尔曼滤波即是计算${\bar{\Sigma }}_t{H}_t^T$第$11\sim 13$行：计算卡尔曼增益、状态均值和方差 无迹卡尔曼滤波算法

扩展卡尔曼滤波算法

4 实例

这里采用扩展卡尔曼滤波的例子实例：雷达监测空中抛物轨迹 从空中位置$(x(0)=0,y(0)=500)$水平抛射出一个物体（初始水平速度为$v_x(0)=50$，初始竖直速度为$v_y(0)=0$）物体受重力$g=9.8$和阻尼力（与速度的平方成正比）的影响水平和竖直阻尼系数分别为$k_x=0.01,k_y=0.05$，不确定度为零均值白噪声$\delta a_x\sim N(0,0.09),\delta a_y\sim N(0,0.09)$在坐标原点处有一雷达，可测得距离$r$,角度$\alpha$, 不确定度为零均值白噪声$\delta r\sim N(0,64),\delta \alpha \sim N(0,0.01)$

状态变量: 物体横向位置$x(k)$，物体横向速度$v_x(k)$，物体纵向位置$y(k)$，物体纵向速度$v_y(k)$ $$X(k)=[x(k),v_x(k),y(k),v_y(k)]$$状态方程： $$\begin{array}{rl}x(k+1)& =x(k)+v_x(k)\cdot T\\ v_x(k+1)& =v_x(k)-(k_x\cdot v_x^2(k)+\delta a_x)\cdot T\\ y(k+1)& =y(k)+v_y(k)\cdot T\\ v_y(k+1)& =v_y(k)+(k_y\cdot v_y^2(k)-g+\delta a_y)\cdot T\end{array}$$观测方程： $$\begin{array}{rl}r(k)& =\sqrt{x(k{)}^2+y(k{)}^2}+\delta r\\ \alpha (k)& =\arctan \frac{x(k)}{y(k)}+\delta \alpha \end{array}$$仿真时间总为$t=15$，采样周期$T=0.1$无迹卡尔曼滤波（UKF）与扩展卡尔曼滤波（EKF）效果如图：

上图可以看出无迹卡尔曼滤波也达到良好的滤波效果，但是为了更好地与扩展卡尔曼滤波进行比较，给出两种滤波方法在$X$和$Y$两个方向的误差图, 可以看出UKF略优于EFK

matlab 代码 clear; close all; clc %% 初值设定 x_0 = 0; %初始x位置 y_0 = 500; %初始y位置 v_x_0 = 50; %初始x速度 v_y_0 = 0; %初始y速度 g = 9.8; % 重力加速度 k_x = 0.01; % 阻尼系数 k_y = 0.05; % 阻尼系数 sigma_a_x = 0.09; % 状态转移不确定度方差 sigma_a_y = 0.09; % 状态转移不确定度方差 sigma_r = 64; % 测量不确定度方差 sigma_alpha = 0.01; % 测量不确定度方差 t = 15; % 仿真时间 T = 0.1; % 采样周期 len = fix(t/T); % 仿真步数 %% 真实轨迹 X = zeros(len, 4); X(1,:) = [x_0, v_x_0, y_0, v_y_0]; for k = 2 : len x_k = X(k-1,1); v_x_k = X(k-1,2); y_k = X(k-1,3); v_y_k = X(k-1,4); x_k = x_k + v_x_k * T; v_x_k = v_x_k - (k_x*v_x_k^2 + sqrt(sigma_a_x)*randn(1,1)) * T; y_k = y_k + v_y_k * T; v_y_k = v_y_k + (k_y*v_y_k^2 - g + sqrt(sigma_a_y)*randn(1,1)) * T; X(k,:) = [x_k, v_x_k, y_k, v_y_k]; end X_temp = X; %% 雷达测量 Z = zeros(len, 2); for k = 1 : len x_k = X(k,1); y_k = X(k,3); r = sqrt(x_k^2 + y_k^2) + sqrt(sigma_r)*randn(1,1); alpha = atan(x_k / y_k) * 180 / pi + sqrt(sigma_alpha)*randn(1,1); Z(k,:) = [r, alpha]; end %% UKF 无迹卡尔曼滤波 n = 4; % 状态变量的数量 alpha_lambda = 1; k_lambda = 1; % 尺度参数，决定σ点距离均值的远近 lambda = alpha_lambda^2 * (n + k_lambda) - n; % 用于计算σ点 chi_t_1 = zeros(2*n + 1, n); % σ点 chi_t = zeros(2*n + 1, n); % σ点 w_m = zeros(2*n + 1, 1); % 计算用于计算均值 w_m(1, 1) = lambda / (n + lambda); w_m(2:end, 1) = 1 / (2*(n + lambda)); w_c = zeros(2*n + 1, 1); % 计算用于计算方差 w_c(1,1) = lambda / (n + lambda) + (1 - alpha_lambda^2 + 2); w_c(2:end, 1) = 1 / (2*(n + lambda)); R_k = diag([0; sigma_a_x; 0; sigma_a_y]); % 状态转移误差的协方差矩阵 Q_k = diag([sigma_r, sigma_alpha]); % 测量函数误差的协方差矩阵 sigma_k = 10 * eye(n); % 最优状态协方差矩阵 sigma_bar_k= 10 * eye(n); % 预测状态协方差矩阵 mu_k = [0, 40, 400, 0]'; % 最优状态均值矩阵 mu_bar_k = zeros(n,1); % 预测的状态均值矩阵 z_k = zeros(4,1); % 观测矩阵 X_UKF_est = zeros(len,n); % UKF后的状态存储 for k = 1 : len %% 1 状态预测 % line: 2 temp = sqrtm((n + lambda) * sigma_k); for i = 1 : 2 * n + 1 if i == 1 chi_t_1(i,:) = mu_k'; elseif i <= n+ 1 chi_t_1(i,:) = mu_k' + real(temp(i - 1, :)); else chi_t_1(i,:) = mu_k' - real(temp(i - n - 1, :)); end end % line: 3 chi_star = zeros(2*n + 1, n); chi_star(:,1) = chi_t_1(:,1) + chi_t_1(:,2)*T; chi_star(:,2) = chi_t_1(:,2) - (k_x*chi_t_1(:,2).^2)*T; chi_star(:,3) = chi_t_1(:,3) + chi_t_1(:,4)*T; chi_star(:,4) = chi_t_1(:,4) + (k_y*chi_t_1(:,4).^2 - g)*T; % line: 4 mu_bar_k = chi_star' * w_m; % line: 5 temp = zeros(n,n); for i = 1 : 2 * n + 1 temp = temp + w_c(i, 1) * (chi_star(i,:)' - mu_bar_k) * (chi_star(i,:)' - mu_bar_k)'; end sigma_bar_k = temp + R_k; %% 2 观测更新 % line: 6 temp = sqrtm((n + lambda) * sigma_k); for i = 1 : 2 * n + 1 if i == 1 chi_t(i,:) = mu_bar_k'; elseif i <= n+ 1 chi_t(i,:) = mu_bar_k' + real(temp(i - 1, :)); else chi_t(i,:) = mu_bar_k' - real(temp(i - n - 1, :)); end end % line: 7 x_k = chi_t(:,1); y_k = chi_t(:,3); r = sqrt(x_k.^2 + y_k.^2); alpha = atan(x_k ./ y_k) * 180 / pi; Z_k = [r,alpha] ; % line: 8 z_k = w_m' * Z_k; % line: 9 temp = zeros(2,2); for i = 1 : 2 * n + 1 temp = temp + w_c(i, 1) * (Z_k(i,:) - z_k)' * (Z_k(i,:) - z_k); end S_k = temp + Q_k; % line: 10 Sigma_x_z = zeros(4,2); for i = 1 : 2 * n + 1 Sigma_x_z = Sigma_x_z + w_c(i, 1) * (chi_t(i,:)' - mu_bar_k) * (Z_k(i,:) - z_k); end % line: 11 K_k = Sigma_x_z * S_k^(-1); % line: 12 mu_k = mu_bar_k + K_k * (Z(k,:) - z_k)'; % line: 13 sigma_k = sigma_bar_k - K_k * S_k * K_k'; %% 3 存储EKF后的值 X_UKF_est(k,:) = mu_k; end %% EKF 扩展卡尔曼滤波 R_k = diag([0; sigma_a_x; 0; sigma_a_y]); % 状态转移误差的协方差矩阵 Q_k = diag([sigma_r, sigma_alpha]); % 测量函数误差的协方差矩阵 sigma_k = 10 * eye(4); % 最优状态协方差矩阵 sigma_bar_k= 10 * eye(4); % 预测状态协方差矩阵 mu_k = [0, 40, 400, 0]'; % 最优状态均值矩阵 mu_bar_k = zeros(4,1); % 预测的状态均值矩阵 z_k = zeros(4,1); % 观测矩阵 X_EKF_est = zeros(len,4); %EKF后的状态存储 for k = 1 : len % 1 状态预测 x1 = mu_k(1) + mu_k(2)*T; v_x1 = mu_k(2) - (k_x*mu_k(2)^2)*T; y1 = mu_k(3) + mu_k(4)*T; v_y1 = mu_k(4) + (k_y*mu_k(4)^2 - g)*T; mu_bar_k = [x1; v_x1; y1; v_y1]; % 预测的均值 G_k = zeros(4,4); % 状态雅可比矩阵 G_k(1,1) = 1; G_k(1,2) = T; G_k(2,2) = 1 - 2*k_x*v_x1*T; G_k(3,3) = 1; G_k(3,4) = T; G_k(4,4) = 1 + 2*k_y*v_y1*T; sigma_bar_k = G_k * sigma_k * G_k' + R_k; % 2 观测更新 r = sqrt(x1*x1+y1*y1); alpha = atan(x1/y1)*180/pi; z_k = [r, alpha]'; % h(\bar{\mu}_t) H_k = zeros(2,4); % 状态雅可比矩阵 x = mu_k(1); y = mu_k(3); H_k(1,1) = x / r; H_k(1,3) = y / r; H_k(2,1) = (1/y)/(1 + (x/y)^2); H_k(2,3) = (-x/(y^2))/(1 + (x/y)^2); K_k = sigma_bar_k * H_k' * (H_k * sigma_bar_k * H_k' + Q_k)^(-1); mu_k = mu_bar_k + K_k * (Z(k,:)' - z_k); sigma_k = (eye(4) - K_k * H_k) * sigma_bar_k; % 3 存储EKF后的值 X_EKF_est(k,:) = mu_k; end %% 绘图 figure, hold on, grid on; plot(X(:,1),X(:,3),'-g'); %真实位置 plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180),'-b'); %观测位置 plot(X_UKF_est(:,1),X_UKF_est(:,3), 'c'); %UKF最优位置 plot(X_EKF_est(:,1),X_EKF_est(:,3), 'r'); %EKF最优位置 xlabel('X'); ylabel('Y'); title('UKF simulation'); legend('real', 'measurement', 'UKF', 'EKF'); axis([-5,230,290,530]); figure, hold on, grid on; T_plot = linspace(0, T, len); plot(T_plot, abs(X(:,1) - X_UKF_est(:,1)),'-c'); %UKF X误差 plot(T_plot, abs(X(:,1) - X_EKF_est(:,1)), '-r'); %EKF X误差 xlabel('t'); ylabel('X error'); title('UKF EKF X error simulation'); legend('UKF X error', 'EKF X error'); figure, hold on, grid on; plot(T_plot, abs(X(:,3) - X_UKF_est(:,3)),'-c'); %UKF Y误差 plot(T_plot, abs(X(:,3) - X_EKF_est(:,3)), '-r'); %EKF Y误差 xlabel('t'); ylabel('Y error'); title('UKF EKF Y error simulation'); legend('UKF Y error', 'EKF Y error');

5 总结

无迹卡尔曼滤波优缺点： 缺点： 计算速度慢于扩展卡尔曼滤波对于近似是高斯分布的场合，滤波效果较好；但是对于置信度的非高斯分布的场合，滤波效果较差 优点： 对于线性系统，滤波效果与卡尔曼滤波相同对于非线性系统，无迹卡尔曼滤波效果优于扩展卡尔曼滤波无需计算雅克比矩阵（对于雅克比矩阵很难求得的场合也适用）

6 参考文献

Thrun, & Sebastian. (2005). Probabilistic robotics.