《最优状态估计-卡尔曼，H∞及非线性滤波》：第14章 无迹卡尔曼滤波
《最优状态估计-卡尔曼，H∞及非线性滤波》：第14章 无迹卡尔曼滤波
- 前言
- 1. MATLAB仿真：示例14.1
- 2. MATLAB仿真：示例14.2
- 3. MATLAB仿真：示例14.3
- 4. 小结
前言
《最优状态估计-卡尔曼，H∞及非线性滤波》由国外引进的一本关于状态估计的专业书籍，2006年正式出版，作者是Dan Simon教授，来自克利夫兰州立大学，电气与计算机工程系。主要应用于运动估计与控制，学习本文的过程中需要有一定的专业基础知识打底。
本书共分为四个部分，全面介绍了最优状态估计的理论和方法。第1部分为基础知识，回顾了线性系统、概率论和随机过程相关知识，介绍了最小二乘法、维纳滤波、状态的统计特性随时间的传播过程。第2部分详细介绍了卡尔曼滤波及其等价形式，介绍了卡尔曼滤波的扩展形式，包括相关噪声和有色噪声条件下的卡尔曼滤波、稳态滤波、衰减记忆滤波和带约束的卡尔曼滤波等（掌握了卡尔曼，基本上可以说这本书掌握了一半）。第3部分详细介绍了H∞滤波，包括时域和频域的H∞滤波，混合卡尔曼/H∞滤波，带约束的H∞ 滤波。第4部分介绍非线性系统滤波方法，包括扩展卡尔曼滤波、无迹卡尔曼滤波及粒子滤波。本书适合作为最优状态估计相关课程的高年级本科生或研究生教材，或从事相关研究工作人员的参考书。
其实自己研究生期间的主研方向并不是运动控制，但自己在本科大三时参加过智能车大赛，当时是采用PID对智能车的运动进行控制，彼时凭借着自学的一知半解，侥幸拿到了奖项。到了研究生期间，实验室正好有研究平衡车的项目，虽然自己不是那个方向，但实验室经常有组内报告，所以对运动控制在实际项目中的应用也算有了基本的了解。参加工作后，有需要对运动估计与控制进行相关研究，所以接触到这本书。
这次重新捡起运动控制，是希望自己可以将这方面的知识进行巩固再学习，结合原书的习题例程进行仿真，简单记录一下这个过程。主要以各章节中习题仿真为主，这是本书的第十四章的3个仿真示例（仿真平台：32位MATLAB2015b），话不多说，开始！
1. MATLAB仿真：示例14.1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例14.1: UnscentedEx.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function UnscentedEx
x = [0; 1]; % x,y coordinates of target
r = sqrt(x(1)^2 + x(2)^2);
theta = atan2(x(2), x(1)); % radians
rand('state',sum(100*clock));
% uniform distribution
rtilde = 0.01;
thetatilde = 0.35; % radians
sigmar = rtilde / sqrt(3);
sigmatheta = thetatilde / sqrt(3);
zarr = [r; theta];
for k = 1 : 300
z = [r + 2 * rtilde * (rand-0.5); theta + 2 * thetatilde * (rand-0.5)];
zarr = [zarr z];
end
close all;
figure;
xarr = zarr(1,:) .* cos(zarr(2,:));
yarr = zarr(1,:) .* sin(zarr(2,:));
plot(xarr, yarr, '.');
xbar = 0;
ybar = sin(thetatilde) / thetatilde;
hold;
plot(xbar, ybar, 'ro');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('y_1'); ylabel('y_2');
figure;
plot(xarr, yarr, '.');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('y_1'); ylabel('y_2');
hold;
plot(xbar, ybar, 'ro');
Px = 0.5 * (1 + sigmar^2) * (1 - sin(2 * thetatilde) / 2 / thetatilde);
Px = sqrt(Px);
Py = 0.5 * (1 + sigmar^2) * (1 + sin(2 * thetatilde) / 2 / thetatilde) - (sin(thetatilde) / thetatilde)^2;
Py = sqrt(Py);
% plot the true uncertainty ellipse
xarr = [];
yarr1 = [];
yarr2 = [];
for x = xbar-Px : Px / 100 : xbar+Px
xarr = [xarr x];
y = ybar + Py * sqrt(1 - ((x - xbar) / Px)^2);
yarr1 = [yarr1 y];
y = ybar - Py * sqrt(1 - ((x - xbar) / Px)^2);
yarr2 = [yarr2 y];
end
plot(xbar, ybar, 'ro');
plot(xarr, yarr1, 'r');
plot(xarr, yarr2, 'r');
% plot the linearized uncertainty ellipse
xbar = 0;
ybar = 1;
Px = sigmatheta;
Py = sigmar;
xarr = [];
yarr1 = [];
yarr2 = [];
for x = xbar-Px : Px / 100 : xbar+Px
xarr = [xarr x];
y = ybar + Py * sqrt(1 - ((x - xbar) / Px)^2);
yarr1 = [yarr1 y];
y = ybar - Py * sqrt(1 - ((x - xbar) / Px)^2);
yarr2 = [yarr2 y];
end
plot(xbar, ybar, 'kx');
plot(xarr, yarr1, 'k');
plot(xarr, yarr2, 'k');
% unscented transformation
clear x;
x(:,1) = [1 + sigmar * sqrt(2) ; pi/2];
x(:,2) = [1 ; pi/2 + sigmatheta * sqrt(2)];
x(:,3) = [1 - sigmar * sqrt(2) ; pi/2];
x(:,4) = [1 ; pi/2 - sigmatheta * sqrt(2)];
z(:,1) = [0 ; 1 + sigmar * sqrt(2)];
z(:,2) = [cos(pi/2+sigmatheta*sqrt(2)) ; sin(pi/2+sigmatheta*sqrt(2))];
z(:,3) = [0 ; 1 - sigmar * sqrt(2)];
z(:,4) = [cos(pi/2-sigmatheta*sqrt(2)) ; sin(pi/2-sigmatheta*sqrt(2))];
zbar = 1/4 * (z(:,1) + z(:,2) + z(:,3) + z(:,4));
Sigmaz = 0;
for i = 1 : 4
Sigmaz = Sigmaz + 1/4 * (z(:,i) - zbar) * (z(:,i) - zbar)';
end
Px = sqrt(Sigmaz(1,1));
Py = sqrt(Sigmaz(2,2));
% plot the unscented transformation
xarr = [];
yarr1 = [];
yarr2 = [];
for x = zbar(1)-Px : Px / 100 : zbar(1)+Px
xarr = [xarr x];
y = zbar(2) + Py * sqrt(1 - ((x - zbar(1)) / Px)^2);
yarr1 = [yarr1 y];
y = zbar(2) - Py * sqrt(1 - ((x - zbar(1)) / Px)^2);
yarr2 = [yarr2 y];
end
plot(zbar(1), zbar(2), 'mx');
plot(xarr, yarr1, 'm');
plot(xarr, yarr2, 'm');
2. MATLAB仿真：示例14.2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例14.2: HybridUKF.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function HybridUKF
% Hybrid extended Kalman filter example.
% Track a body falling through the atmosphere.
% This example is taken from [Jul00], which was based on [Ath68].
rho0 = 2; % lb-sec^2/ft^4
g = 32.2; % ft/sec^2
k = 2e4; % ft
R = 10^4; % measurement noise variance (ft^2)
Q = diag([0 0 0]); % process noise covariance
M = 10^5; % horizontal range of position sensor
a = 10^5; % altitude of position sensor
x = [3e5; -2e4; 1e-3];
xhat = [3e5; -2e4; 1e-3];
xhatukf = xhat;
P = diag([1e6 4e6 10]);
Pukf = P;
T = 0.5; % measurement time step
randn('state',sum(100*clock)); % random number generator seed
tf = 30; % simulation length (seconds)
dt = 0.001; % time step for integration (seconds)
xArray = x;
xhatArray = xhat;
xhatukfArray = xhatukf;
Parray = diag(P);
Pukfarray = diag(Pukf);
W = ones(6,1) / 6; % UKF weights
for t = T : T : tf
% Simulate the system.
for tau = dt : dt : T
xdot(1,1) = x(2);
xdot(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 * x(3) - g;
xdot(3,1) = 0;
xdot = xdot + sqrt(dt * Q) * [randn; randn; randn];
x = x + xdot * dt;
end
% Simulate the noisy measurement.
z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn;
% Simulate the continuous-time part of the Kalman filter (time update).
for tau = dt : dt : T
xhatdot(1,1) = xhat(2);
xhatdot(2,1) = rho0 * exp(-xhat(1)/k) * xhat(2)^2 / 2 * xhat(3) - g;
xhatdot(3,1) = 0;
xhat = xhat + xhatdot * dt;
F = [0 1 0; -rho0 * exp(-xhat(1)/k) * xhat(2)^2 / 2 / k * xhat(3) ...
rho0 * exp(-xhat(1)/k) * xhat(2) * xhat(3) ...
rho0 * exp(-xhat(1)/k) * xhat(2)^2 / 2; ...
0 0 0];
H = [(xhat(1) - a) / sqrt(M^2 + (xhat(1)-a)^2) 0 0];
Pdot = F * P + P * F' + Q * dt - P * H' * inv(R) * H * P;
P = P + Pdot * dt;
end
% Simulate the discrete-time part of the Kalman filter (measurement update).
H = [(xhat(1) - a) / sqrt(M^2 + (xhat(1)-a)^2) 0 0];
K = P * H' * inv(H * P * H' + R);
zhat = sqrt(M^2 + (xhat(1)-a)^2);
xhat = xhat + K * (z - zhat);
P = (eye(3) - K * H) * P * (eye(3) - K * H)' + K * R * K';
% Generate the UKF sigma points.
[root,p] = chol(3*Pukf);
for i = 1 : 3
sigma(:,i) = xhatukf + root(i,:)';
sigma(:,i+3) = xhatukf - root(i,:)';
end
for i = 1 : 6
xbreve(:,i) = sigma(:,i);
end
% UKF time update
for i = 1 : 6
for tau = dt : dt : T
xbrevedot(1,1) = xbreve(2,i);
xbrevedot(2,1) = rho0 * exp(-xbreve(1,i)/k) * xbreve(2,i)^2 / 2 * xbreve(3,i) - g;
xbrevedot(3,1) = 0;
xbreve(:,i) = xbreve(:,i) + xbrevedot * dt;
end
end
xhatukf = zeros(3,1);
for i = 1 : 6
xhatukf = xhatukf + W(i) * xbreve(:,i);
end
Pukf = zeros(3,3);
for i = 1 : 6
Pukf = Pukf + W(i) * (xbreve(:,i) - xhatukf) * (xbreve(:,i) - xhatukf)';
end
Pukf = Pukf + Q;
% UKF measurement update
for i = 1 : 6
zukf(:,i) = sqrt(M^2 + (xbreve(1,i)-a)^2);
end
zhat = 0;
for i = 1 : 6
zhat = zhat + W(i) * zukf(:,i);
end
Py = 0;
Pxy = zeros(3,1);
for i = 1 : 6
Py = Py + W(i) * (zukf(:,i) - zhat) * (zukf(:,i) - zhat)';
Pxy = Pxy + W(i) * (xbreve(:,i) - xhat) * (zukf(:,i) - zhat)';
end
Py = Py + R;
Kukf = Pxy * inv(Py);
xhatukf = xhatukf + Kukf * (z - zhat);
Pukf = Pukf - Kukf * Py * Kukf';
% Save data for plotting.
xArray = [xArray x];
xhatArray = [xhatArray xhat];
xhatukfArray = [xhatukfArray xhatukf];
Parray = [Parray diag(P)];
Pukfarray = [Pukfarray diag(Pukf)];
end
close all;
t = 0 : T : tf;
figure;
semilogy(t, abs(xArray(1,:) - xhatArray(1,:)), 'b'); hold;
%plot(t, sqrt(Parray(1,:)), 'b--');
semilogy(t, abs(xArray(1,:) - xhatukfArray(1,:)), 'r:');
%plot(t, sqrt(Pukfarray(1,:)), 'r--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Position Estimation Error');
legend('Kalman filter', 'Unscented filter');
figure;
semilogy(t, abs(xArray(2,:) - xhatArray(2,:)), 'b'); hold;
%plot(t, sqrt(Parray(2,:)), 'b--');
semilogy(t, abs(xArray(2,:) - xhatukfArray(2,:)), 'r:');
%plot(t, sqrt(Pukfarray(2,:)), 'r--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Velocity Estimation Error');
legend('Kalman filter', 'Unscented filter');
figure;
semilogy(t, abs(xArray(3,:) - xhatArray(3,:)), 'b'); hold;
%plot(t, sqrt(Parray(3,:)), 'b--');
semilogy(t, abs(xArray(3,:) - xhatukfArray(3,:)), 'r:');
%plot(t, sqrt(Pukfarray(3,:)), 'r--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Ballistic Coefficient Estimation Error');
legend('Kalman filter', 'Unscented filter');
figure;
plot(t, xArray(1,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Position');
figure;
plot(t, xArray(2,:));
title('Falling Body Simulation', 'FontSize', 12);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Velocity');
3. MATLAB仿真：示例14.3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例14.3: HybridSimplex.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [EstErrK, EstErrUs, EstErrU] = HybridSimplex(SphericalFlag)
% Optimal State Estimation, by Dan Simon
%
% Hybrid unscented Kalman filter example with simplex sigma points or spherical sigma points.
% Track a body falling through the atmosphere.
% This example is taken from [Jul00], which was based on [Ath68].
% INPUTS:
% SphericalFlag = flag indicating whether or not to use spherical sigma
% points (if false, then use simplex sigma points)
% OUTPUTS:
% EstErrK = Kalman filter RMS est error for position, velocity, ballistic coefficient
% EstErrUs = Standard UKF RMS est error for position, velocity, ballistic coefficient
% EstErrU = Minimal (spherical or simplex) UKF RMS est error for position, velocity, ballistic coefficient
if ~exist('SphericalFlag', 'var')
SphericalFlag = true;
end
rho0 = 2; % lb-sec^2/ft^4
g = 32.2; % ft/sec^2
k = 2e4; % ft
R = 10^4; % measurement noise variance (ft^2)
Q = diag([0 0 0]); % process noise covariance
M = 10^5; % horizontal range of position sensor
a = 10^5; % altitude of position sensor
x = [3e5; -2e4; 1000];
xhat = 1.01 * [3e5; -2e4; 1000]; % EKF estimate
xhatukf = xhat; % simplex or spherical UKF estimate
xhatukfs = xhat; % standard UKF estimate
P = diag(0.1*abs(x));
Pukf = P;
Pukfs = P;
T = 0.5; % measurement time step
randn('state',sum(100*clock)); % random number generator seed
tf = 30; % simulation length (seconds)
dt = 0.001; % time step for integration (seconds)
xArray = x;
xhatArray = xhat;
xhatukfArray = xhatukf;
xhatukfsArray = xhatukfs;
N = length(x);
% Generate the weights for the simplex or spherical UKF.
if SphericalFlag
W(1) = 0;
W(2) = (1 - W(1)) / (N + 1);
W(3) = W(2);
W(4) = W(3);
W(5) = W(4);
else
W(1) = 0;
W(2) = (1 - W(1)) / 2^N;
W(3) = W(2);
W(4) = 2 * W(3);
W(5) = 2 * W(4);
end
% Generate the sigma points for the simplex or spherical UKF.
% Initialization
sigma = zeros(3,5);
sigma(1,1) = 0;
sigma(1,2) = -1 / sqrt(2 * W(2));
sigma(1,3) = 1 / sqrt(2 * W(2));
% j = 2 iteration
j = 2;
if SphericalFlag
sigma(1:j,1) = [sigma(1:j-1,1) ; 0];
sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(j*(j+1)*W(2))];
sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(j*(j+1)*W(2))];
sigma(1:j,4) = [zeros(j-1,1) ; j/sqrt(j*(j+1)*W(2))];
else
sigma(1:j,1) = [sigma(1:j-1,1) ; 0];
sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(2*W(j+2))];
sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(2*W(j+2))];
sigma(1:j,4) = [zeros(j-1,1) ; 1/sqrt(2*W(j+2))];
end
% j = 3 iteration
j = 3;
if SphericalFlag
sigma(1:j,1) = [sigma(1:j-1,1) ; 0];
sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(j*(j+1)*W(2))];
sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(j*(j+1)*W(2))];
sigma(1:j,4) = [sigma(1:j-1,4) ; -1/sqrt(j*(j+1)*W(2))];
sigma(1:j,5) = [zeros(j-1,1) ; j/sqrt(j*(j+1)*W(2))];
else
sigma(1:j,1) = [sigma(1:j-1,1) ; 0];
sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(2*W(j+2))];
sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(2*W(j+2))];
sigma(1:j,4) = [sigma(1:j-1,4) ; -1/sqrt(2*W(j+2))];
sigma(1:j,5) = [zeros(j-1,1) ; 1/sqrt(2*W(j+2))];
end
Ws = ones(2*N,1) / 2 / N; % standard UKF weights
for t = T : T : tf
% Simulate the system.
for tau = dt : dt : T
xdot(1,1) = x(2);
xdot(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 / x(3) - g;
xdot(3,1) = 0;
xdot = xdot + sqrt(dt * Q) * [randn; randn; randn];
x = x + xdot * dt;
end
% Simulate the noisy measurement.
z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn;
% Simulate the continuous-time part of the Kalman filter (time update).
for tau = dt : dt : T
xhatdot(1,1) = xhat(2);
xhatdot(2,1) = rho0 * exp(-xhat(1)/k) * xhat(2)^2 / 2 / xhat(3) - g;
xhatdot(3,1) = 0;
xhat = xhat + xhatdot * dt;
temp = rho0 * exp(-xhat(1)/k) * xhat(2) / xhat(3);
F = [0 1 0; -temp * xhat(2) / 2 / k temp ...
-temp * xhat(2) / 2 / xhat(3); ...
0 0 0];
H = [(xhat(1) - a) / sqrt(M^2 + (xhat(1)-a)^2) 0 0];
Pdot = F * P + P * F' + Q * dt - P * H' * inv(R) * H * P;
P = P + Pdot * dt;
end
% Simulate the discrete-time part of the Kalman filter (measurement update).
H = [(xhat(1) - a) / sqrt(M^2 + (xhat(1)-a)^2) 0 0];
K = P * H' * inv(H * P * H' + R);
zhat = sqrt(M^2 + (xhat(1)-a)^2);
xhat = xhat + K * (z - zhat);
P = (eye(N) - K * H) * P * (eye(N) - K * H)' + K * R * K';
% Start of simplex / spherical UKF - matrix square root calculation
[root,p] = chol(Pukf);
for i = 1 : N+2
xbreve(:,i) = xhatukf + root * sigma(:,i);
end
% Simplex / spherical UKF time update
for i = 1 : N+2
for tau = dt : dt : T
xbrevedot(1,1) = xbreve(2,i);
xbrevedot(2,1) = rho0 * exp(-xbreve(1,i)/k) * xbreve(2,i)^2 / 2 / xbreve(3,i) - g;
xbrevedot(3,1) = 0;
xbreve(:,i) = xbreve(:,i) + xbrevedot * dt;
end
end
xhatukf = zeros(N,1);
for i = 1 : N+2
xhatukf = xhatukf + W(i) * xbreve(:,i);
end
Pukf = Q;
for i = 1 : N+2
Pukf = Pukf + W(i) * (xbreve(:,i) - xhatukf) * (xbreve(:,i) - xhatukf)';
end
% Simplex / spherical UKF measurement update
for i = 1 : N+2
zukf(:,i) = sqrt(M^2 + (xbreve(1,i)-a)^2);
end
zhat = 0;
for i = 1 : N+2
zhat = zhat + W(i) * zukf(:,i);
end
Py = R;
Pxy = zeros(N,1);
for i = 1 : N+2
Py = Py + W(i) * (zukf(:,i) - zhat) * (zukf(:,i) - zhat)';
Pxy = Pxy + W(i) * (xbreve(:,i) - xhatukf) * (zukf(:,i) - zhat)';
end
Kukf = Pxy * inv(Py);
xhatukf = xhatukf + Kukf * (z - zhat);
Pukf = Pukf - Kukf * Py * Kukf';
% Start of standard UKF - generate the UKF sigma points.
[root,p] = chol(N*Pukfs);
for i = 1 : N
sigmas(:,i) = xhatukfs + root(i,:)';
sigmas(:,i+N) = xhatukfs - root(i,:)';
end
for i = 1 : 2*N
xbreve(:,i) = sigmas(:,i);
end
% Standard UKF time update
for i = 1 : 2*N
for tau = dt : dt : T
xbrevedot(1,1) = xbreve(2,i);
xbrevedot(2,1) = rho0 * exp(-xbreve(1,i)/k) * xbreve(2,i)^2 / 2 / xbreve(3,i) - g;
xbrevedot(3,1) = 0;
xbreve(:,i) = xbreve(:,i) + xbrevedot * dt;
end
end
xhatukfs = zeros(N,1);
for i = 1 : 2*N
xhatukfs = xhatukfs + Ws(i) * xbreve(:,i);
end
Pukfs = Q;
for i = 1 : 2*N
Pukfs = Pukfs + Ws(i) * (xbreve(:,i) - xhatukfs) * (xbreve(:,i) - xhatukfs)';
end
% Standard UKF measurement update
for i = 1 : 2*N
zukf(:,i) = sqrt(M^2 + (xbreve(1,i)-a)^2);
end
zhat = 0;
for i = 1 : 2*N
zhat = zhat + Ws(i) * zukf(:,i);
end
Py = R;
Pxy = zeros(N,1);
for i = 1 : 2*N
Py = Py + Ws(i) * (zukf(:,i) - zhat) * (zukf(:,i) - zhat)';
Pxy = Pxy + Ws(i) * (xbreve(:,i) - xhatukfs) * (zukf(:,i) - zhat)';
end
Kukf = Pxy * inv(Py);
xhatukfs = xhatukfs + Kukf * (z - zhat);
Pukfs = Pukfs - Kukf * Py * Kukf';
% Save data for plotting.
xArray = [xArray x];
xhatArray = [xhatArray xhat];
xhatukfArray = [xhatukfArray xhatukf];
xhatukfsArray = [xhatukfsArray xhatukfs];
end
EstErrK = std(xArray' - xhatArray');
EstErrU = std(xArray' - xhatukfArray');
EstErrUs = std(xArray' - xhatukfsArray');
disp(['RMS altitude est err = ', num2str(EstErrK(1)), ' (EKF), ', num2str(EstErrUs(1)), ...
' (Std UKF), ', num2str(EstErrU(1)), ' (Minimal UKF)']);
disp(['RMS velocity est err = ', num2str(EstErrK(2)), ' (EKF), ', num2str(EstErrUs(2)), ...
' (Std UKF), ', num2str(EstErrU(2)), ' (Minimal UKF)']);
disp(['RMS ball coeff est err = ', num2str(EstErrK(3)), ' (EKF), ', num2str(EstErrUs(3)), ...
' (Std UKF), ', num2str(EstErrU(3)), ' (Minimal UKF)']);
>> HybridSimplex
RMS altitude est err = 603.3477 (EKF), 447.8848 (Std UKF), 572.3741 (Minimal UKF)
RMS velocity est err = 170.0054 (EKF), 107.5279 (Std UKF), 140.4361 (Minimal UKF)
RMS ball coeff est err = 10.4088 (EKF), 6.4646 (Std UKF), 0.46956 (Minimal UKF)
ans =
603.3477 170.0054 10.4088
4. 小结
运动控制在现代生活中的实际应用非常广泛，除了智能工厂中各种智能设备的自动运转控制，近几年最火的自动驾驶技术，以及航空航天领域，都缺少不了它的身影，所以熟练掌握状态估计理论，对未来就业也是非常有帮助的。切记矩阵理论与概率论等知识的基础一定要打好。对本章内容感兴趣或者想充分学习了解的，建议去研习书中第十四章节的内容，有条件的可以通过习题的联系进一步巩固充实。后期会对其中一些知识点在自己理解的基础上进行讨论补充，欢迎大家一起学习交流。
原书链接：Optimal State Estimation:Kalman, H-infinity, and Nonlinear Approaches