《最优状态估计-卡尔曼,H∞及非线性滤波》:第12章 H∞滤波器的其他问题
前言
《最优状态估计-卡尔曼,H∞及非线性滤波》由国外引进的一本关于状态估计的专业书籍,2006年正式出版,作者是Dan Simon教授,来自克利夫兰州立大学,电气与计算机工程系。主要应用于运动估计与控制,学习本文的过程中需要有一定的专业基础知识打底。
本书共分为四个部分,全面介绍了最优状态估计的理论和方法。第1部分为基础知识,回顾了线性系统、概率论和随机过程相关知识,介绍了最小二乘法、维纳滤波、状态的统计特性随时间的传播过程。第2部分详细介绍了卡尔曼滤波及其等价形式,介绍了卡尔曼滤波的扩展形式,包括相关噪声和有色噪声条件下的卡尔曼滤波、稳态滤波、衰减记忆滤波和带约束的卡尔曼滤波等(掌握了卡尔曼,基本上可以说这本书掌握了一半)。第3部分详细介绍了H∞滤波,包括时域和频域的H∞滤波,混合卡尔曼/H∞滤波,带约束的H∞ 滤波。第4部分介绍非线性系统滤波方法,包括扩展卡尔曼滤波、无迹卡尔曼滤波及粒子滤波。本书适合作为最优状态估计相关课程的高年级本科生或研究生教材,或从事相关研究工作人员的参考书。
其实自己研究生期间的主研方向并不是运动控制,但自己在本科大三时参加过智能车大赛,当时是采用PID对智能车的运动进行控制,彼时凭借着自学的一知半解,侥幸拿到了奖项。到了研究生期间,实验室正好有研究平衡车的项目,虽然自己不是那个方向,但实验室经常有组内报告,所以对运动控制在实际项目中的应用也算有了基本的了解。参加工作后,有需要对运动估计与控制进行相关研究,所以接触到这本书。
这次重新捡起运动控制,是希望自己可以将这方面的知识进行巩固再学习,结合原书的习题例程进行仿真,简单记录一下这个过程。主要以各章节中习题仿真为主,这是本书的第十二章的4个仿真示例(仿真平台:32位MATLAB2015b),话不多说,开始!
1. MATLAB仿真:示例12.1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能:《最优状态估计-卡尔曼,H∞及非线性滤波》示例仿真
%示例12.1: AddHinfEx1.m
%环境:Win7,Matlab2015b
%Modi: C.S
%时间:2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function AddHinfEx1
Q = 1;
R = 1;
thetaMin = 0;
thetaMax = 1;
dtheta = 0.01;
KArr = [];
PArr = [];
for theta = thetaMin : dtheta : thetaMax
c(1) = theta^2 - theta^4 * R;
c(2) = Q * theta^4 * R - Q * theta^2 + R * theta^2 - 1;
c(3) = Q * (1 - 2 * theta^2 * R);
c(4) = Q * R;
Pall = roots(c);
% Find a real positive root of the ARE that results in a stable estimator.
P = inf;
for i = 1 : length(Pall)
if abs(theta^2 * Pall(i) - 1) < 1e-12
continue;
end
Pa = Pall(i) / (1 - theta^2 * Pall(i));
V = R + Pa;
Fhat = 1 - Pa / V;
if isreal(Pall(i)) && (Pall(i) >= 0) && (Pall(i) < P) && (abs(Fhat) < 1)
P = Pall(i);
K = Pa / V;
end
end
if P == inf
thetaMax = theta - dtheta;
break;
end
PArr = [PArr P];
KArr = [KArr K];
end
close all;
theta = thetaMin : dtheta : thetaMax;
figure;
plot(theta, KArr);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('H_\infty performance bound \theta'); ylabel('Estimator gain K');
figure;
plot(theta, PArr);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('H_\infty performance bound \theta'); ylabel('Kalman performance bound P');
2. MATLAB仿真:示例12.2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能:《最优状态估计-卡尔曼,H∞及非线性滤波》示例仿真
%示例12.2: AddHinfEx3.m
%环境:Win7,Matlab2015b
%Modi: C.S
%时间:2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function AddHinfEx3
% Robust Kalman filtering with model uncertainties.
% Based on paper by Hung and Fang.
J = 10; % motor moment of inertia
JFilter = 100 * J; % assumed moment of inertia
%JFilter = J;
Friction = 40; % viscous friction coefficient
c = 5; % voltage-to-torque constant
alpha = Friction / J;
alphaFilter = Friction / JFilter;
k = c / J;
u = 0; % input voltage
sigmaw = 2; % std dev of torque disturbance
sigmav = 0.2; % std dev of angle measurement noise (degrees)
sigmav = sigmav * pi / 180;
tf = 10; % simulation time (seconds)
%tf = 40;
A = [0 1; 0 -alpha]; % continuous time system matrix
AFilter = [0 1; 0 -alphaFilter]; % assumed system matrix
B = [0; c/J]; % continuous time input matrix
Bw = [0; 1/JFilter]; % continuous time noise input matrix
Q = Bw * sigmaw * Bw'; % process noise covariance
H = [1 0]; % measurement matrix
x0 = [0; 10]; % initial state
% Continuous time simulation
dt = 0.01;
x = x0;
yArr = [];
for t = 0 : dt : tf
xdot = A * x + B * u;
x = x + xdot * dt;
y = H * x;
yArr = [yArr y];
end
% Discrete time simulation
T = 0.1;
Ad = expm(A * T); % discretized system matrix
AdFilter = expm(AFilter * T); % assumed discretized system matrix
Bd = c / Friction * [T - 1/alpha + exp(-alpha*T)/alpha ; 1 - exp(-alpha*T)]; % discretized input matrix
Bwd = 1 / Friction * [T - 1/alpha + exp(-alpha*T)/alpha ; 1 - exp(-alpha*T)]; % discretized noise input matrix
x = x0;
xhat = x;
Pplus = Q;
ydArr = [];
xtildeArr = [];
% Robust filter parameters
N = .00001*eye(2);
M1 = eye(2);
M2 = [0 0];
D1 = [Bwd'; 0 0]';
D2 = [0 1];
theta = .6;
alpha = .10;
eps = 1e-8;
S1 = 1e4*eye(2);
S2 = 1e4*eye(2);
Q1 = S1;
Q2 = S2;
R11 = D1 * D1' + alpha * M1 * M1';
R12 = D1 * D2' + alpha * M1 * M2';
R22 = D2 * D2' + alpha * M2 * M2';
L = .001*eye(2);
xhatr = x;
xrobustArr = [];
for t = 0 : T : tf;
% System and measurement simulation
y = H * x + sigmav * randn;
x = Ad * x + Bd * u + Bwd * sigmaw * randn;
% Kalman filter
Pminus = AdFilter * Pplus * AdFilter' + Q;
K = Pminus * H' * inv(H * Pminus * H' + sigmav^2);
xhat = AdFilter * xhat + Bd * u;
xtildeArr = [xtildeArr x-xhat];
xhat = xhat + K * (y - H * xhat);
Pplus = (eye(2) - K * H) * Pminus * (eye(2) - K * H)' + K * sigmav^2 * K';
% Save data for plotting
ydArr = [ydArr y];
% Robust filter equations
A = AdFilter;
R1 = inv(inv(Q2) - N' * N / alpha) * A';
R2 = inv(R1) * inv(inv(Q2) - N' * N / alpha) * inv(R1');
A1 = A + R11 * inv(R1);
C1 = H + R12' * inv(R1);
temp1 = inv(inv(Q1) - theta^2 * L' * L);
R = C1 * temp1 * C1' + R12' * R2 * R12 + R22;
% Robust state estimate
G = (A1 * temp1 * C1' + R11 * R2 * R12 + R12) * inv(R);
F = A1 - G * C1;
xhatr = F * xhatr + G * y;
xrobustArr = [xrobustArr x-xhatr];
% Riccati equations for robust filter
Q1 = A1 * temp1 * A1' + R11 + R11 * R2 * R11' - ...
(A1 * temp1 * C1' + R11 * R2 * R12 + R12) * inv(R) * (A1 * temp1 * C1' + R11 * R2 * R12 + R12)' + eps * eye(2);
Q2 = A * Q2 * A' + A * Q2 * N' * inv(alpha * eye(2) - N * Q2 * N') * N * Q2 * A' + R11 + eps * eye(2);
% Check conditions for validity of robust state estimate
lambda = eig(Q1);
for i = 1 : 2
if ~isreal(lambda(i)) || (lambda(i) <= 0)
disp(['Q1 is not positive definite - t = ', num2str(t)]);
return;
end
end
lambda = eig(eye(2) / theta^2 - L * Q1 * L');
for i = 1 : 2
if ~isreal(lambda(i)) || (lambda(i) <= 0)
disp(['Q1 condition not satisfied - t = ', num2str(t)]);
return;
end
end
lambda = eig(Q2);
for i = 1 : 2
if ~isreal(lambda(i)) || (lambda(i) <= 0)
disp(['Q2 is not positive definite - t = ', num2str(t)]);
return;
end
end
lambda = eig(alpha * eye(2) - N * Q2 * N');
for i = 1 : 2
if ~isreal(lambda(i)) || (lambda(i) <= 0)
disp(['Q2 condition not satisfied - t = ', num2str(t)]);
return;
end
end
end
close all;
t = 0 : dt : tf;
td = 0 : T : tf;
%figure;
%plot(t, yArr, td, ydArr);
%legend('Continuous time', 'Discrete time');
xtildeArr = xtildeArr * 180 / pi;
xrobustArr = xrobustArr * 180 / pi;
figure;
plot(td, xtildeArr(1,:), 'r:', td, xrobustArr(1,:), 'b-');
set(gca,'FontSize',12); set(gcf,'Color','White');
legend('Kalman filter', 'Robust filter');
xlabel('seconds'); ylabel('deg');
figure;
plot(td, xtildeArr(2,:), 'r:', td, xrobustArr(2,:), 'b-');
set(gca,'FontSize',12); set(gcf,'Color','White');
legend('Kalman filter', 'Robust filter');
xlabel('seconds'); ylabel('deg/sec');
% Compute RMS estimation errors
iStart = -1;
for i = 1 : size(xtildeArr,2)
if (abs(xtildeArr(1,i)) <= 1) && (iStart < 0)
iStart = i;
elseif abs(xtildeArr(1,i)) > 1
iStart = -1;
end
end
len = size(xtildeArr,2) - iStart + 1;
KalmanRMSPos = sqrt(norm(xtildeArr(1,iStart:end))^2 / len);
KalmanRMSVel = sqrt(norm(xtildeArr(2,iStart:end))^2 / len);
iStart = -1;
for i = 1 : size(xrobustArr,2)
if (abs(xrobustArr(1,i)) <= 1) && (iStart < 0)
iStart = i;
elseif abs(xrobustArr(1,i)) > 1
iStart = -1;
end
end
len = size(xrobustArr,2) - iStart + 1;
RobustRMSPos = sqrt(norm(xrobustArr(1,iStart:end))^2 / len);
RobustRMSVel = sqrt(norm(xrobustArr(2,iStart:end))^2 / len);
disp(['Kalman filter RMS estimation errors = ', num2str(KalmanRMSPos), ', ', num2str(KalmanRMSVel)]);
disp(['Robust filter RMS estimation errors = ', num2str(RobustRMSPos), ', ', num2str(RobustRMSVel)]);
disp(['Q1 = diag(', num2str(Q1(1,1)), ', ', num2str(Q1(2,2)), ')']);
>> AddHinfEx3
Kalman filter RMS estimation errors = 0.38006, 1.2028
Robust filter RMS estimation errors = 0.42315, 1.4598
Q1 = diag(0.49198, 1.3397)
3. MATLAB仿真:示例12.3(1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能:《最优状态估计-卡尔曼,H∞及非线性滤波》示例仿真
%示例12.3: AddHinfConstr.m
%环境:Win7,Matlab2015b
%Modi: C.S
%时间:2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ErrKarray, ErrKCarray, ErrHinfarray, ErrHinfCarray] = AddHinfConstr(g, T, tf)
% function AddHinfConstr
% This m-file simulates a vehicle tracking problem.
% The vehicle state is estimated with a minimax filter.
% In addition, with the a priori knowledge that the vehicle is on
% a particular road, the vehicle state is estimated with a
% constrained minimax filter.
% This m-file also simulates a Kalman filter and constrained
% Kalman filter so you can compare results.
% The state consists of the north and east position, and the
% north and east velocity of the vehicle.
% The measurement consists of north and east position.
% For further details see the web site
% http://www.csuohio.edu/simond/minimaxconstrained/.
% INPUTS
% g = gamma (I suggest 40)
% T = time step in seconds (I suggest 1)
% tf = final time in seconds (I suggest 120)
% OUTPUTS
% ErrKarray = time varying array of error of Kalman unconstrained state estimate
% ErrKCarray = time varying array of error of Kalman constrained state estimate
% ErrHinfarray = time varying array of error of Minimax unconstrained state estimate
% ErrHinfCarray = time varying array of error of Minimax constrained state estimate
if ~exist('g', 'var')
g = 40;
end
if ~exist('T', 'var')
T = 1;
end
if ~exist('tf', 'var')
tf = 120;
end
Q = diag([4, 4, 1, 1]); % Process noise covariance (m, m, m/sec, m/sec)
Qsqrt = sqrt(Q);
R = diag([900, 900]); % Measurement noise covariance (m, m)
Rsqrt = sqrt(R);
theta = pi / 3; % heading angle (measured CCW from east)
tantheta = tan(theta);
% Define the initial state x, initial unconstrained Kalman filter estimate xhat,
% and initial constrained Kalman filter estimate xtilde.
x = [0; 0; tantheta; 1] * 100;
xhat = x;
xtilde = x;
P = diag([R(1,1), R(2,2), Q(1,1), Q(2,2)]); % Initial estimation error covariance
% AccelDecelFlag is used to simulate the vehicle alternately accelerating and
% decelerating, as if in traffic.
AccelDecelFlag = 1;
% System matrix.
A = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1];
% Input matrix.
B = [0; 0; T*sin(theta); T*cos(theta)];
% Normalized measurement matrix.
C = inv(Rsqrt) * [1 0 0 0; 0 1 0 0];
% State constraint matrices.
D = [1 -tantheta 0 0; 0 0 1 -tantheta];
% Normalize D so that D*D'=I.
D = D / sqrt(1 + tantheta^2);
V = D' * D;
d = [0; 0];
% Initialize arrays for saving data for plotting.
xarray = [];
xhatarray = [];
xtildearray = [];
randn('state', sum(100*clock));
% Minimax initialization.
% Make sure that xtildeinf satisfies the state constraint.
Qbar = P;
Qtilde = P;
xhatinf = x;
xtildeinf = x;
xhatinfarray = [];
xtildeinfarray = [];
for t = T : T : tf
% Get the noise-corrupted measurement z.
z = C * x;
MeasErr = randn(size(z));
z = z + MeasErr;
% Set the known input u.
if AccelDecelFlag == 1
if (x(3) > 30) | (x(4) > 30)
AccelDecelFlag = -1;
end
else
if (x(3) < 5) | (x(4) < 5)
AccelDecelFlag = 1;
end
end
u = 1 * AccelDecelFlag;
% Run the unconstrained minimax filter.
Pinf = inv(eye(4) - Qbar / g / g + Qbar * C' * C) * Qbar;
Qbar = A * Pinf * A' + Q;
K = A * Pinf * C';
headinghat = atan2(xhatinf(3), xhatinf(4));
Bhat = [0; 0; T*sin(headinghat); T*cos(headinghat)];
xhatinf = A * xhatinf + Bhat * u + K * (z - C * xhatinf);
xhatinfarray = [xhatinfarray xhatinf];
% Run the constrained minimax filter.
Ptilde = inv(eye(4) - Qtilde / g / g + Qtilde * C' * C) * Qtilde;
Qtilde = (eye(4) - V) * A * Ptilde * A' * (eye(4) - V) + Q;
lambda = eig(eye(4) - Qtilde / g / g);
for i = 1 : 4
if ~isreal(lambda(i)) || lambda(i) <= 0
disp(['positive semidefinite condition fails - t = ', num2str(t)]);
return;
end
end
K = (eye(4) - V) * A * Pinf * C';
headingtilde = atan2(xtildeinf(3), xtildeinf(4));
Btilde = [0; 0; T*sin(headingtilde); T*cos(headingtilde)];
xtildeinf = A * xtildeinf + Btilde * u + K * (z - C * xtildeinf);
xtildeinfarray = [xtildeinfarray xtildeinf];
% Run the unconstrained Kalman filter.
K = A * P * C' * inv(C * P * C' + eye(size(R)));
% Update the state estimation error covariance.
P = (A * P - K * C * P) * A' + Q;
% Estimate the heading on the basis of the state estimate.
headinghat = atan2(xhat(3), xhat(4));
Bhat = [0; 0; T*sin(headinghat); T*cos(headinghat)];
xhat = A * xhat + Bhat * u + K * (z - C * xhat);
xhatarray = [xhatarray xhat];
% Find the constrained Kalman filter estimate.
xtilde = xhat - D' * inv(D*D') * (D * xhat - d);
xtildearray = [xtildearray xtilde];
% Simulate the system dynamics.
x = A * x + B * u + Qsqrt * randn(size(x));
% Uncomment the following line to add unmodeled process noise.
x = x + [0; 0; 1; 1];
% Constrain the vehicle (i.e., the true state) to the straight road.
if abs(x(1) - tantheta * x(2)) > 2
x(2) = (x(2) + x(1) * tantheta) / (1 + tantheta^2);
x(1) = x(2) * tantheta;
end
if abs(x(3) - tantheta * x(4)) > 0.2
x(4) = (x(4) + x(3) * tantheta) / (1 + tantheta^2);
x(3) = x(4) * tantheta;
end
xarray = [xarray x];
end
% Compute average position estimation errors.
EstError = xarray - xhatarray;
EstError = sqrt(EstError(1,:).^2 + EstError(2,:).^2);
EstError = mean(EstError);
disp(['Average Kalman Unconstrained Position Estimation Error = ', num2str(EstError)]);
EstErrorConstr = xarray - xtildearray;
EstErrorConstr = sqrt(EstErrorConstr(1,:).^2 + EstErrorConstr(2,:).^2);
EstErrorConstr = mean(EstErrorConstr);
disp(['Average Kalman Constrained Position Estimation Error (W=I) = ', num2str(EstErrorConstr)]);
EstError = xarray - xhatinfarray;
EstError = sqrt(EstError(1,:).^2 + EstError(2,:).^2);
EstError = mean(EstError);
disp(['Average Minimax Unconstrained Position Estimation Error = ', num2str(EstError)]);
EstErrorConstr = xarray - xtildeinfarray;
EstErrorConstr = sqrt(EstErrorConstr(1,:).^2 + EstErrorConstr(2,:).^2);
EstErrorConstr = mean(EstErrorConstr);
disp(['Average Minimax Constrained Position Estimation Error = ', num2str(EstErrorConstr)]);
disp(' ');
% Compute average velocity estimation errors.
EstError = xarray - xhatarray;
EstError = sqrt(EstError(3,:).^2 + EstError(4,:).^2);
EstError = mean(EstError);
disp(['Average Kalman Unconstrained Velocity Estimation Error = ', num2str(EstError)]);
EstErrorConstr = xarray - xtildearray;
EstErrorConstr = sqrt(EstErrorConstr(3,:).^2 + EstErrorConstr(4,:).^2);
EstErrorConstr = mean(EstErrorConstr);
disp(['Average Kalman Constrained Velocity Estimation Error (W=I) = ', num2str(EstErrorConstr)]);
EstError = xarray - xhatinfarray;
EstError = sqrt(EstError(3,:).^2 + EstError(4,:).^2);
EstError = mean(EstError);
disp(['Average Minimax Unconstrained Velocity Estimation Error = ', num2str(EstError)]);
EstErrorConstr = xarray - xtildeinfarray;
EstErrorConstr = sqrt(EstErrorConstr(3,:).^2 + EstErrorConstr(4,:).^2);
EstErrorConstr = mean(EstErrorConstr);
disp(['Average Minimax Constrained Velocity Estimation Error = ', num2str(EstErrorConstr)]);
close all;
t = 0 : T : tf-T;
% Plot the position errors.
figure;
plot(t, xarray(1, :), ':', t, xarray(2, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('True Position');
xlabel('seconds');
ylabel('meters');
figure;
plot(t, xarray(1, :) - xhatarray(1, :), ':', ...
t, xarray(2, :) - xhatarray(2, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Kalman Position Estimation Error (Unconstrained)');
xlabel('seconds');
ylabel('meters');
figure;
plot(t, xarray(1, :) - xtildearray(1, :), ':', ...
t, xarray(2, :) - xtildearray(2, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Kalman Position Estimation Error (Constrained)');
xlabel('seconds');
ylabel('meters');
figure;
plot(t, xarray(1, :) - xhatinfarray(1, :), ':', ...
t, xarray(2, :) - xhatinfarray(2, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Minimax Position Estimation Error (Unconstrained)');
xlabel('seconds');
ylabel('meters');
figure;
plot(t, xarray(1, :) - xtildeinfarray(1, :), ':', ...
t, xarray(2, :) - xtildeinfarray(2, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Minimax Position Estimation Error (Constrained)');
xlabel('seconds');
ylabel('meters');
% Plot the velocity errors.
figure;
plot(t, xarray(3, :), ':', t, xarray(4, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('True Velocity');
xlabel('seconds');
ylabel('meters/sec');
figure;
plot(t, xarray(3, :) - xhatarray(3, :), ':', ...
t, xarray(4, :) - xhatarray(4, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Kalman Velocity Estimation Error (Unconstrained)');
xlabel('seconds');
ylabel('meters/sec');
figure;
plot(t, xarray(3, :) - xtildearray(3, :), ':', ...
t, xarray(4, :) - xtildearray(4, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Kalman Velocity Estimation Error (Constrained)');
xlabel('seconds');
ylabel('meters/sec');
figure;
plot(t, xarray(3, :) - xhatinfarray(3, :), ':', ...
t, xarray(4, :) - xhatinfarray(4, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Minimax Velocity Estimation Error (Unconstrained)');
xlabel('seconds');
ylabel('meters/sec');
figure;
plot(t, xarray(3, :) - xtildeinfarray(3, :), ':', ...
t, xarray(4, :) - xtildeinfarray(4, :), '-');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Minimax Velocity Estimation Error (Constrained)');
xlabel('seconds');
ylabel('meters/sec');
% Compute estimation errors
ErrKarray = xarray - xhatarray; % Kalman unconstrained
ErrKCarray = xarray - xtildearray; % Kalman constrained
ErrHinfarray = xarray - xhatinfarray; % Minimax unconstrained
ErrHinfCarray = xarray - xtildeinfarray; % Minimax constrained
>> AddHinfConstr
Average Kalman Unconstrained Position Estimation Error = 47.3031
Average Kalman Constrained Position Estimation Error (W=I) = 45.8958
Average Minimax Unconstrained Position Estimation Error = 33.808
Average Minimax Constrained Position Estimation Error = 29.9959
Average Kalman Unconstrained Velocity Estimation Error = 12.2025
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.1632
Average Minimax Unconstrained Velocity Estimation Error = 13.951
Average Minimax Constrained Velocity Estimation Error = 13.8882
ans =
1 至 11 列
2.1021 4.6576 -5.5752 1.2412 6.1456 12.9223 16.9510 28.2017 33.5239 19.6266 22.7117
-13.7298 -18.5117 -27.7775 -17.2401 -0.8873 1.1309 0.9678 0.6048 -3.6149 -0.1231 5.4318
1.7145 1.6501 3.1169 4.0731 6.0558 8.1921 9.2998 10.7172 10.3581 8.8803 10.1442
0.9899 0.8889 1.6816 2.5251 4.5353 5.5151 5.8235 5.7658 4.8365 5.5483 6.5053
12 至 22 列
38.5219 44.4403 45.6600 32.3203 26.1107 27.9899 26.7130 43.9324 50.2026 36.8780 23.9276
13.7067 11.4012 12.5970 11.1471 10.4840 19.9277 22.2339 32.3548 28.5536 32.9007 19.3047
11.8099 12.1150 10.7565 8.4365 8.9234 9.2642 8.3803 10.4749 10.6847 8.6611 7.3470
7.3709 6.8018 6.1360 5.3922 6.1116 7.0001 6.8208 7.7319 6.8347 6.9213 5.2786
23 至 33 列
35.7340 27.0067 35.7621 31.2704 21.8421 36.6601 33.5776 14.2047 28.7950 33.4945 37.2525
32.8166 32.6025 31.5390 19.2637 18.4413 21.2496 10.3983 5.0878 6.0921 18.9643 22.4857
10.2206 9.3677 8.7679 9.9017 9.5305 10.7796 10.9041 9.2398 10.9769 10.1138 10.1121
7.5430 7.2722 6.1560 5.6142 5.9226 5.9596 5.0585 4.9648 5.1154 5.8830 6.0266
34 至 44 列
53.4615 47.6331 57.7633 52.4818 52.0899 42.4684 39.5301 43.5750 52.5319 37.3535 38.9976
30.1241 31.7957 36.2823 32.6750 40.1442 28.8425 28.8101 22.8377 26.3387 27.8391 21.6822
11.9573 11.9813 11.4413 9.1422 9.0179 7.7047 8.9127 10.0138 10.5023 8.5426 10.0893
6.8797 7.4152 6.9337 5.5191 6.2477 4.8344 5.5697 5.2367 5.3600 5.3788 5.5922
45 至 55 列
29.9134 34.8805 31.0782 36.8187 34.2104 51.4473 57.7444 62.0251 64.9898 63.0157 61.2184
19.8744 8.5271 10.8948 31.8807 35.0911 32.4642 33.5704 18.4961 23.7674 33.3524 19.6395
9.0906 11.6994 12.1159 12.4703 12.0466 13.9547 14.9279 15.2163 15.8326 15.1058 15.2460
5.3686 5.2873 6.2025 8.3573 8.6199 8.1153 8.4060 6.6391 7.6372 8.5787 7.2730
56 至 66 列
51.4435 52.2292 64.8432 61.8267 72.4265 67.1666 60.0418 64.1710 61.2112 50.2227 50.7115
16.7285 12.4733 9.7459 12.4310 29.1149 35.5906 37.4959 38.0821 27.5967 23.4768 30.6288
14.3816 14.5446 15.2707 14.4852 14.0534 13.6803 13.0066 12.2274 11.2242 11.5250 11.5913
7.2532 6.9474 6.4259 6.7917 7.8403 8.5971 8.8674 8.1571 6.4229 6.8494 7.6799
67 至 77 列
36.1670 47.0458 32.5690 44.9724 41.4482 35.2633 29.6032 36.0244 21.9522 9.1811 12.5438
38.1006 38.9814 40.9926 47.3041 41.4155 21.0808 32.7359 33.6360 24.6206 13.7084 4.7936
8.5835 9.0816 7.4875 8.3998 7.3538 5.5449 5.3322 5.6208 4.2151 3.0870 3.9381
7.5318 6.8777 6.9115 7.0410 5.9025 2.7228 4.3251 4.1362 3.0057 1.8967 1.1731
78 至 88 列
10.2133 22.1923 -2.7621 -5.2805 0.7776 13.8549 35.4462 22.0967 48.7753 45.2256 44.7766
6.4212 9.0596 7.5518 13.4287 9.5941 14.4834 15.4371 20.7913 20.6572 27.4704 12.7220
5.3329 6.9302 4.3048 6.1273 7.3201 9.6695 12.9848 11.7959 14.4561 13.8337 13.7724
2.5369 2.9456 2.8747 4.7984 4.6177 5.5467 6.1906 7.2282 6.8685 7.6337 6.0622
89 至 99 列
56.1649 59.4393 61.3578 66.2799 66.2936 72.3698 73.0649 79.7958 67.0197 53.0138 59.3050
13.6381 19.4806 34.2514 35.2398 36.7303 51.8570 64.6003 45.3835 37.9727 37.9738 39.6058
14.9076 15.0867 15.7574 16.4857 16.5243 17.6907 15.9992 14.9772 12.6833 12.4928 13.8703
6.2214 7.1253 9.2644 9.4040 9.6035 11.5559 11.8020 8.3474 7.0537 7.8047 8.3641
100 至 110 列
62.6832 55.1541 70.3775 60.8956 45.1279 34.8041 45.7233 37.9280 29.3489 27.4273 31.4650
44.2537 36.5204 35.1888 37.8775 31.9712 34.7701 39.7342 41.0283 15.9528 16.8911 16.6437
16.0908 14.7242 15.2960 12.9476 10.9613 10.2379 11.2655 9.3568 9.3869 8.5573 9.4299
9.9037 8.6691 7.8595 7.5192 6.7213 7.3419 7.5084 6.9323 4.4298 4.4076 4.6873
111 至 120 列
37.0928 47.3920 45.3219 35.8659 35.9376 29.7946 40.0132 25.3517 32.5171 18.5284
6.7734 8.6330 11.8804 22.9830 28.6661 37.0225 39.1462 16.2844 13.3094 14.9225
9.7833 10.7714 10.6188 10.9605 9.4465 8.8125 10.0736 7.8691 8.1790 6.0827
3.6339 3.9422 4.6139 6.8201 6.5242 7.2816 7.3394 4.1759 3.7221 3.6606
4. MATLAB仿真:示例12.3(2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能:《最优状态估计-卡尔曼,H∞及非线性滤波》示例仿真
%示例12.3: AddHinfConstrMonte.m
%环境:Win7,Matlab2015b
%Modi: C.S
%时间:2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function AddHinfConstrMonte
% Obtain Monte Carlo based statistics for the unconstrained and constrained
% Kalman and H-infinity state estimates
nMonte = 100; % number of Monte Carlo simulations
for i = 1 : nMonte
[ErrKarray(i,:,:), ErrKCarray(i,:,:), ErrHinfarray(i,:,:), ErrHinfCarray(i,:,:)] = AddHinfConstr;
end
% At each time step compute the RMS position and velocity estimation errors
% at each time step.
nTime = size(ErrKarray,3); % number of time steps in each simulation
ErrKPos = zeros(1, nTime); % position error of unconstrained Kalman filter
ErrKVel = ErrKPos; % velocity error of unconstrained Kalman filter
ErrKCPos = ErrKPos; % position error of constrained Kalman filter
ErrKCVel = ErrKPos; % velocity error of constrained Kalman filter
ErrHinfPos = ErrKPos; % position error of unconstrained H-infinity filter
ErrHinfVel = ErrKPos; % velocity error of unconstrained H-infinity filter
ErrHinfCPos = ErrKPos; % position error of constrained H-infinity filter
ErrHinfCVel = ErrKPos; % velocity error constrained H-infinity filter
for t = 1 : nTime
for i = 1 : nMonte
ErrKPos(t) = ErrKPos(t) + ErrKarray(i,1,t)^2 + ErrKarray(i,2,t)^2;
ErrKVel(t) = ErrKVel(t) + ErrKarray(i,3,t)^2 + ErrKarray(i,4,t)^2;
ErrKCPos(t) = ErrKCPos(t) + ErrKCarray(i,1,t)^2 + ErrKCarray(i,2,t)^2;
ErrKCVel(t) = ErrKCVel(t) + ErrKCarray(i,3,t)^2 + ErrKCarray(i,4,t)^2;
ErrHinfPos(t) = ErrHinfPos(t) + ErrHinfarray(i,1,t)^2 + ErrHinfarray(i,2,t)^2;
ErrHinfVel(t) = ErrHinfVel(t) + ErrHinfarray(i,3,t)^2 + ErrHinfarray(i,4,t)^2;
ErrHinfCPos(t) = ErrHinfCPos(t) + ErrHinfCarray(i,1,t)^2 + ErrHinfCarray(i,2,t)^2;
ErrHinfCVel(t) = ErrHinfCVel(t) + ErrHinfCarray(i,3,t)^2 + ErrHinfCarray(i,4,t)^2;
end
ErrKPos(t) = sqrt( ErrKPos(t) / nMonte ) ;
ErrKVel(t) = sqrt( ErrKVel(t) / nMonte ) ;
ErrKCPos(t) = sqrt( ErrKCPos(t) / nMonte ) ;
ErrKCVel(t) = sqrt( ErrKCVel(t) / nMonte ) ;
ErrHinfPos(t) = sqrt( ErrHinfPos(t) / nMonte ) ;
ErrHinfVel(t) = sqrt( ErrHinfVel(t) / nMonte ) ;
ErrHinfCPos(t) = sqrt( ErrHinfCPos(t) / nMonte ) ;
ErrHinfCVel(t) = sqrt( ErrHinfCVel(t) / nMonte ) ;
end
% Plot the results
close all;
t = 1 : nTime;
figure;
plot(t, ErrKPos, 'r-', t, ErrKCPos, 'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Kalman filter position estimation error');
legend('unconstrained', 'constrained');
xlabel('seconds'); ylabel('meters');
figure;
plot(t, ErrKVel, 'r-', t, ErrKCVel, 'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('Kalman filter velocity estimation error');
legend('unconstrained', 'constrained');
xlabel('seconds'); ylabel('meters/s');
figure;
plot(t, ErrHinfPos, 'r-', t, ErrHinfCPos, 'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('H_{\infty} filter position estimation error');
legend('unconstrained', 'constrained');
xlabel('seconds'); ylabel('meters');
figure;
plot(t, ErrHinfVel, 'r-', t, ErrHinfCVel, 'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
title('H_{\infty} filter velocity estimation error');
legend('unconstrained', 'constrained');
xlabel('seconds'); ylabel('meters/s');
disp('average RMS position estimation errors:');
disp([num2str(mean(ErrKPos)), ', unconstrained Kalman filter']);
disp([num2str(mean(ErrKCPos)), ', constrained Kalman filter']);
disp([num2str(mean(ErrHinfPos)), ', unconstrained H_\infty filter']);
disp([num2str(mean(ErrHinfCPos)), ', constrained H_\infty filter']);
disp('average RMS velocity estimation errors:');
disp([num2str(mean(ErrKVel)), ', unconstrained Kalman filter']);
disp([num2str(mean(ErrKCVel)), ', constrained Kalman filter']);
disp([num2str(mean(ErrHinfVel)), ', unconstrained H_\infty filter']);
disp([num2str(mean(ErrHinfCVel)), ', constrained H_\infty filter']);
>> AddHinfConstrMonte
Average Kalman Unconstrained Position Estimation Error = 51.2244
Average Kalman Constrained Position Estimation Error (W=I) = 49.5575
Average Minimax Unconstrained Position Estimation Error = 37.1505
Average Minimax Constrained Position Estimation Error = 32.9276
Average Kalman Unconstrained Velocity Estimation Error = 12.3274
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.2692
Average Minimax Unconstrained Velocity Estimation Error = 14.1563
Average Minimax Constrained Velocity Estimation Error = 14.0774
Average Kalman Unconstrained Position Estimation Error = 49.0222
Average Kalman Constrained Position Estimation Error (W=I) = 46.3153
Average Minimax Unconstrained Position Estimation Error = 36.6294
Average Minimax Constrained Position Estimation Error = 29.9749
Average Kalman Unconstrained Velocity Estimation Error = 13.0306
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.9476
Average Minimax Unconstrained Velocity Estimation Error = 14.8234
Average Minimax Constrained Velocity Estimation Error = 14.7094
Average Kalman Unconstrained Position Estimation Error = 47.4407
Average Kalman Constrained Position Estimation Error (W=I) = 44.1987
Average Minimax Unconstrained Position Estimation Error = 34.6502
Average Minimax Constrained Position Estimation Error = 28.6482
Average Kalman Unconstrained Velocity Estimation Error = 11.6402
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.5339
Average Minimax Unconstrained Velocity Estimation Error = 13.3036
Average Minimax Constrained Velocity Estimation Error = 13.1884
Average Kalman Unconstrained Position Estimation Error = 42.4885
Average Kalman Constrained Position Estimation Error (W=I) = 40.713
Average Minimax Unconstrained Position Estimation Error = 31.1301
Average Minimax Constrained Position Estimation Error = 26.3908
Average Kalman Unconstrained Velocity Estimation Error = 11.6205
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.5807
Average Minimax Unconstrained Velocity Estimation Error = 13.329
Average Minimax Constrained Velocity Estimation Error = 13.2627
Average Kalman Unconstrained Position Estimation Error = 47.4314
Average Kalman Constrained Position Estimation Error (W=I) = 45.2006
Average Minimax Unconstrained Position Estimation Error = 36.8776
Average Minimax Constrained Position Estimation Error = 31.2981
Average Kalman Unconstrained Velocity Estimation Error = 11.7402
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.6698
Average Minimax Unconstrained Velocity Estimation Error = 13.3871
Average Minimax Constrained Velocity Estimation Error = 13.2832
Average Kalman Unconstrained Position Estimation Error = 50.3497
Average Kalman Constrained Position Estimation Error (W=I) = 47.9243
Average Minimax Unconstrained Position Estimation Error = 37.4314
Average Minimax Constrained Position Estimation Error = 32.1402
Average Kalman Unconstrained Velocity Estimation Error = 11.9858
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.901
Average Minimax Unconstrained Velocity Estimation Error = 13.7454
Average Minimax Constrained Velocity Estimation Error = 13.6428
Average Kalman Unconstrained Position Estimation Error = 44.5146
Average Kalman Constrained Position Estimation Error (W=I) = 42.9215
Average Minimax Unconstrained Position Estimation Error = 32.1751
Average Minimax Constrained Position Estimation Error = 27.2548
Average Kalman Unconstrained Velocity Estimation Error = 12.3054
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.2575
Average Minimax Unconstrained Velocity Estimation Error = 14.0666
Average Minimax Constrained Velocity Estimation Error = 13.9884
Average Kalman Unconstrained Position Estimation Error = 49.6138
Average Kalman Constrained Position Estimation Error (W=I) = 47.631
Average Minimax Unconstrained Position Estimation Error = 36.1011
Average Minimax Constrained Position Estimation Error = 31.5008
Average Kalman Unconstrained Velocity Estimation Error = 11.9322
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.8646
Average Minimax Unconstrained Velocity Estimation Error = 13.713
Average Minimax Constrained Velocity Estimation Error = 13.6253
Average Kalman Unconstrained Position Estimation Error = 46.9027
Average Kalman Constrained Position Estimation Error (W=I) = 44.8889
Average Minimax Unconstrained Position Estimation Error = 33.7234
Average Minimax Constrained Position Estimation Error = 28.7933
Average Kalman Unconstrained Velocity Estimation Error = 12.4641
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.4039
Average Minimax Unconstrained Velocity Estimation Error = 14.1514
Average Minimax Constrained Velocity Estimation Error = 14.0737
Average Kalman Unconstrained Position Estimation Error = 53.6348
Average Kalman Constrained Position Estimation Error (W=I) = 50.8288
Average Minimax Unconstrained Position Estimation Error = 39.3162
Average Minimax Constrained Position Estimation Error = 32.591
Average Kalman Unconstrained Velocity Estimation Error = 12.9725
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.8833
Average Minimax Unconstrained Velocity Estimation Error = 14.8472
Average Minimax Constrained Velocity Estimation Error = 14.7324
Average Kalman Unconstrained Position Estimation Error = 46.5796
Average Kalman Constrained Position Estimation Error (W=I) = 43.6346
Average Minimax Unconstrained Position Estimation Error = 35.6517
Average Minimax Constrained Position Estimation Error = 29.1051
Average Kalman Unconstrained Velocity Estimation Error = 11.5564
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.4765
Average Minimax Unconstrained Velocity Estimation Error = 13.2915
Average Minimax Constrained Velocity Estimation Error = 13.1884
Average Kalman Unconstrained Position Estimation Error = 44.8967
Average Kalman Constrained Position Estimation Error (W=I) = 41.716
Average Minimax Unconstrained Position Estimation Error = 34.6739
Average Minimax Constrained Position Estimation Error = 27.1885
Average Kalman Unconstrained Velocity Estimation Error = 11.8611
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.7523
Average Minimax Unconstrained Velocity Estimation Error = 13.6194
Average Minimax Constrained Velocity Estimation Error = 13.4839
Average Kalman Unconstrained Position Estimation Error = 43.2944
Average Kalman Constrained Position Estimation Error (W=I) = 41.2465
Average Minimax Unconstrained Position Estimation Error = 31.6293
Average Minimax Constrained Position Estimation Error = 24.8504
Average Kalman Unconstrained Velocity Estimation Error = 12.1072
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.0343
Average Minimax Unconstrained Velocity Estimation Error = 13.9337
Average Minimax Constrained Velocity Estimation Error = 13.8199
Average Kalman Unconstrained Position Estimation Error = 46.098
Average Kalman Constrained Position Estimation Error (W=I) = 43.4772
Average Minimax Unconstrained Position Estimation Error = 33.4605
Average Minimax Constrained Position Estimation Error = 26.615
Average Kalman Unconstrained Velocity Estimation Error = 12.8526
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.7849
Average Minimax Unconstrained Velocity Estimation Error = 14.6561
Average Minimax Constrained Velocity Estimation Error = 14.5683
Average Kalman Unconstrained Position Estimation Error = 45.3813
Average Kalman Constrained Position Estimation Error (W=I) = 42.6082
Average Minimax Unconstrained Position Estimation Error = 33.3441
Average Minimax Constrained Position Estimation Error = 26.8024
Average Kalman Unconstrained Velocity Estimation Error = 12.058
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.9821
Average Minimax Unconstrained Velocity Estimation Error = 13.902
Average Minimax Constrained Velocity Estimation Error = 13.8001
Average Kalman Unconstrained Position Estimation Error = 50.117
Average Kalman Constrained Position Estimation Error (W=I) = 46.5216
Average Minimax Unconstrained Position Estimation Error = 37.193
Average Minimax Constrained Position Estimation Error = 30.3122
Average Kalman Unconstrained Velocity Estimation Error = 12.1782
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.0749
Average Minimax Unconstrained Velocity Estimation Error = 13.9354
Average Minimax Constrained Velocity Estimation Error = 13.8189
Average Kalman Unconstrained Position Estimation Error = 41.5294
Average Kalman Constrained Position Estimation Error (W=I) = 38.5806
Average Minimax Unconstrained Position Estimation Error = 31.4252
Average Minimax Constrained Position Estimation Error = 24.8495
Average Kalman Unconstrained Velocity Estimation Error = 11.5608
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.4749
Average Minimax Unconstrained Velocity Estimation Error = 13.3193
Average Minimax Constrained Velocity Estimation Error = 13.2084
Average Kalman Unconstrained Position Estimation Error = 45.6182
Average Kalman Constrained Position Estimation Error (W=I) = 43.399
Average Minimax Unconstrained Position Estimation Error = 34.0386
Average Minimax Constrained Position Estimation Error = 28.729
Average Kalman Unconstrained Velocity Estimation Error = 11.7203
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.6559
Average Minimax Unconstrained Velocity Estimation Error = 13.4853
Average Minimax Constrained Velocity Estimation Error = 13.3931
Average Kalman Unconstrained Position Estimation Error = 45.4794
Average Kalman Constrained Position Estimation Error (W=I) = 43.2955
Average Minimax Unconstrained Position Estimation Error = 32.7035
Average Minimax Constrained Position Estimation Error = 27.7943
Average Kalman Unconstrained Velocity Estimation Error = 11.6461
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.5861
Average Minimax Unconstrained Velocity Estimation Error = 13.3068
Average Minimax Constrained Velocity Estimation Error = 13.2242
Average Kalman Unconstrained Position Estimation Error = 43.2028
Average Kalman Constrained Position Estimation Error (W=I) = 41.8229
Average Minimax Unconstrained Position Estimation Error = 31.8871
Average Minimax Constrained Position Estimation Error = 27.4482
Average Kalman Unconstrained Velocity Estimation Error = 11.2767
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.2263
Average Minimax Unconstrained Velocity Estimation Error = 12.8817
Average Minimax Constrained Velocity Estimation Error = 12.8013
Average Kalman Unconstrained Position Estimation Error = 48.4671
Average Kalman Constrained Position Estimation Error (W=I) = 46.0574
Average Minimax Unconstrained Position Estimation Error = 35.3957
Average Minimax Constrained Position Estimation Error = 29.3119
Average Kalman Unconstrained Velocity Estimation Error = 13.2926
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.2123
Average Minimax Unconstrained Velocity Estimation Error = 15.1227
Average Minimax Constrained Velocity Estimation Error = 15.0199
Average Kalman Unconstrained Position Estimation Error = 39.9057
Average Kalman Constrained Position Estimation Error (W=I) = 38.0959
Average Minimax Unconstrained Position Estimation Error = 29.673
Average Minimax Constrained Position Estimation Error = 24.2313
Average Kalman Unconstrained Velocity Estimation Error = 10.6383
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.5756
Average Minimax Unconstrained Velocity Estimation Error = 12.1697
Average Minimax Constrained Velocity Estimation Error = 12.0753
Average Kalman Unconstrained Position Estimation Error = 52.1883
Average Kalman Constrained Position Estimation Error (W=I) = 50.4917
Average Minimax Unconstrained Position Estimation Error = 38.3268
Average Minimax Constrained Position Estimation Error = 33.5851
Average Kalman Unconstrained Velocity Estimation Error = 12.6657
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.6119
Average Minimax Unconstrained Velocity Estimation Error = 14.5331
Average Minimax Constrained Velocity Estimation Error = 14.4538
Average Kalman Unconstrained Position Estimation Error = 46.5052
Average Kalman Constrained Position Estimation Error (W=I) = 44.1704
Average Minimax Unconstrained Position Estimation Error = 34.5817
Average Minimax Constrained Position Estimation Error = 28.1746
Average Kalman Unconstrained Velocity Estimation Error = 12.3634
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.2804
Average Minimax Unconstrained Velocity Estimation Error = 14.1042
Average Minimax Constrained Velocity Estimation Error = 13.996
Average Kalman Unconstrained Position Estimation Error = 42.9917
Average Kalman Constrained Position Estimation Error (W=I) = 40.359
Average Minimax Unconstrained Position Estimation Error = 33.0137
Average Minimax Constrained Position Estimation Error = 26.7553
Average Kalman Unconstrained Velocity Estimation Error = 12.2659
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.1946
Average Minimax Unconstrained Velocity Estimation Error = 14.0601
Average Minimax Constrained Velocity Estimation Error = 13.9567
Average Kalman Unconstrained Position Estimation Error = 44.7125
Average Kalman Constrained Position Estimation Error (W=I) = 42.5652
Average Minimax Unconstrained Position Estimation Error = 32.1581
Average Minimax Constrained Position Estimation Error = 27.0006
Average Kalman Unconstrained Velocity Estimation Error = 11.7398
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.6601
Average Minimax Unconstrained Velocity Estimation Error = 13.5433
Average Minimax Constrained Velocity Estimation Error = 13.4403
Average Kalman Unconstrained Position Estimation Error = 54.7549
Average Kalman Constrained Position Estimation Error (W=I) = 52.7721
Average Minimax Unconstrained Position Estimation Error = 39.764
Average Minimax Constrained Position Estimation Error = 34.9725
Average Kalman Unconstrained Velocity Estimation Error = 12.8703
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.7986
Average Minimax Unconstrained Velocity Estimation Error = 14.7765
Average Minimax Constrained Velocity Estimation Error = 14.6776
Average Kalman Unconstrained Position Estimation Error = 49.0335
Average Kalman Constrained Position Estimation Error (W=I) = 47.3782
Average Minimax Unconstrained Position Estimation Error = 35.1672
Average Minimax Constrained Position Estimation Error = 30.4685
Average Kalman Unconstrained Velocity Estimation Error = 12.4624
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.4088
Average Minimax Unconstrained Velocity Estimation Error = 14.3251
Average Minimax Constrained Velocity Estimation Error = 14.2438
Average Kalman Unconstrained Position Estimation Error = 44.1615
Average Kalman Constrained Position Estimation Error (W=I) = 42.2288
Average Minimax Unconstrained Position Estimation Error = 32.8179
Average Minimax Constrained Position Estimation Error = 27.4232
Average Kalman Unconstrained Velocity Estimation Error = 11.8827
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.8114
Average Minimax Unconstrained Velocity Estimation Error = 13.5617
Average Minimax Constrained Velocity Estimation Error = 13.4559
Average Kalman Unconstrained Position Estimation Error = 35.2895
Average Kalman Constrained Position Estimation Error (W=I) = 33.27
Average Minimax Unconstrained Position Estimation Error = 28.1461
Average Minimax Constrained Position Estimation Error = 22.178
Average Kalman Unconstrained Velocity Estimation Error = 9.7802
Average Kalman Constrained Velocity Estimation Error (W=I) = 9.714
Average Minimax Unconstrained Velocity Estimation Error = 11.2331
Average Minimax Constrained Velocity Estimation Error = 11.1258
Average Kalman Unconstrained Position Estimation Error = 41.9737
Average Kalman Constrained Position Estimation Error (W=I) = 38.5194
Average Minimax Unconstrained Position Estimation Error = 31.174
Average Minimax Constrained Position Estimation Error = 24.0818
Average Kalman Unconstrained Velocity Estimation Error = 11.4449
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.3408
Average Minimax Unconstrained Velocity Estimation Error = 13.0726
Average Minimax Constrained Velocity Estimation Error = 12.9592
Average Kalman Unconstrained Position Estimation Error = 43.7074
Average Kalman Constrained Position Estimation Error (W=I) = 41.4387
Average Minimax Unconstrained Position Estimation Error = 32.1523
Average Minimax Constrained Position Estimation Error = 25.3232
Average Kalman Unconstrained Velocity Estimation Error = 11.7513
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.6817
Average Minimax Unconstrained Velocity Estimation Error = 13.5133
Average Minimax Constrained Velocity Estimation Error = 13.4057
Average Kalman Unconstrained Position Estimation Error = 42.8086
Average Kalman Constrained Position Estimation Error (W=I) = 40.657
Average Minimax Unconstrained Position Estimation Error = 31.7183
Average Minimax Constrained Position Estimation Error = 26.9237
Average Kalman Unconstrained Velocity Estimation Error = 11.5689
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.5097
Average Minimax Unconstrained Velocity Estimation Error = 13.2302
Average Minimax Constrained Velocity Estimation Error = 13.1518
Average Kalman Unconstrained Position Estimation Error = 44.2861
Average Kalman Constrained Position Estimation Error (W=I) = 41.7622
Average Minimax Unconstrained Position Estimation Error = 33.1169
Average Minimax Constrained Position Estimation Error = 26.9582
Average Kalman Unconstrained Velocity Estimation Error = 11.417
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.3268
Average Minimax Unconstrained Velocity Estimation Error = 13.101
Average Minimax Constrained Velocity Estimation Error = 12.9926
Average Kalman Unconstrained Position Estimation Error = 49.9207
Average Kalman Constrained Position Estimation Error (W=I) = 47.6372
Average Minimax Unconstrained Position Estimation Error = 37.335
Average Minimax Constrained Position Estimation Error = 31.593
Average Kalman Unconstrained Velocity Estimation Error = 12.1023
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.023
Average Minimax Unconstrained Velocity Estimation Error = 13.8299
Average Minimax Constrained Velocity Estimation Error = 13.722
Average Kalman Unconstrained Position Estimation Error = 44.6474
Average Kalman Constrained Position Estimation Error (W=I) = 41.5922
Average Minimax Unconstrained Position Estimation Error = 34.8272
Average Minimax Constrained Position Estimation Error = 27.2449
Average Kalman Unconstrained Velocity Estimation Error = 12.3971
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.3049
Average Minimax Unconstrained Velocity Estimation Error = 14.3443
Average Minimax Constrained Velocity Estimation Error = 14.2158
Average Kalman Unconstrained Position Estimation Error = 51.7242
Average Kalman Constrained Position Estimation Error (W=I) = 50.0289
Average Minimax Unconstrained Position Estimation Error = 37.6982
Average Minimax Constrained Position Estimation Error = 32.6745
Average Kalman Unconstrained Velocity Estimation Error = 13.1347
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.076
Average Minimax Unconstrained Velocity Estimation Error = 14.8671
Average Minimax Constrained Velocity Estimation Error = 14.7722
Average Kalman Unconstrained Position Estimation Error = 43.9499
Average Kalman Constrained Position Estimation Error (W=I) = 42.737
Average Minimax Unconstrained Position Estimation Error = 32.6835
Average Minimax Constrained Position Estimation Error = 28.024
Average Kalman Unconstrained Velocity Estimation Error = 11.7254
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.6746
Average Minimax Unconstrained Velocity Estimation Error = 13.4204
Average Minimax Constrained Velocity Estimation Error = 13.3308
Average Kalman Unconstrained Position Estimation Error = 49.7745
Average Kalman Constrained Position Estimation Error (W=I) = 48.5317
Average Minimax Unconstrained Position Estimation Error = 35.1324
Average Minimax Constrained Position Estimation Error = 31.1128
Average Kalman Unconstrained Velocity Estimation Error = 13.4984
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.4621
Average Minimax Unconstrained Velocity Estimation Error = 15.3549
Average Minimax Constrained Velocity Estimation Error = 15.2921
Average Kalman Unconstrained Position Estimation Error = 42.5797
Average Kalman Constrained Position Estimation Error (W=I) = 40.5102
Average Minimax Unconstrained Position Estimation Error = 31.7265
Average Minimax Constrained Position Estimation Error = 25.3667
Average Kalman Unconstrained Velocity Estimation Error = 12.4364
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.3653
Average Minimax Unconstrained Velocity Estimation Error = 14.1821
Average Minimax Constrained Velocity Estimation Error = 14.0767
Average Kalman Unconstrained Position Estimation Error = 41.4802
Average Kalman Constrained Position Estimation Error (W=I) = 38.7733
Average Minimax Unconstrained Position Estimation Error = 32.5983
Average Minimax Constrained Position Estimation Error = 26.3705
Average Kalman Unconstrained Velocity Estimation Error = 10.4857
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.4054
Average Minimax Unconstrained Velocity Estimation Error = 11.9877
Average Minimax Constrained Velocity Estimation Error = 11.8784
Average Kalman Unconstrained Position Estimation Error = 47.9056
Average Kalman Constrained Position Estimation Error (W=I) = 45.6381
Average Minimax Unconstrained Position Estimation Error = 33.8942
Average Minimax Constrained Position Estimation Error = 28.4077
Average Kalman Unconstrained Velocity Estimation Error = 12.9763
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.9163
Average Minimax Unconstrained Velocity Estimation Error = 14.7472
Average Minimax Constrained Velocity Estimation Error = 14.6611
Average Kalman Unconstrained Position Estimation Error = 48.2366
Average Kalman Constrained Position Estimation Error (W=I) = 46.3375
Average Minimax Unconstrained Position Estimation Error = 35.2418
Average Minimax Constrained Position Estimation Error = 29.9772
Average Kalman Unconstrained Velocity Estimation Error = 12.0877
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.026
Average Minimax Unconstrained Velocity Estimation Error = 13.8854
Average Minimax Constrained Velocity Estimation Error = 13.7972
Average Kalman Unconstrained Position Estimation Error = 46.4135
Average Kalman Constrained Position Estimation Error (W=I) = 44.1586
Average Minimax Unconstrained Position Estimation Error = 34.8225
Average Minimax Constrained Position Estimation Error = 28.6586
Average Kalman Unconstrained Velocity Estimation Error = 12.4922
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.4139
Average Minimax Unconstrained Velocity Estimation Error = 14.2859
Average Minimax Constrained Velocity Estimation Error = 14.1733
Average Kalman Unconstrained Position Estimation Error = 46.2864
Average Kalman Constrained Position Estimation Error (W=I) = 44.9509
Average Minimax Unconstrained Position Estimation Error = 34.7555
Average Minimax Constrained Position Estimation Error = 30.7297
Average Kalman Unconstrained Velocity Estimation Error = 11.0741
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.0244
Average Minimax Unconstrained Velocity Estimation Error = 12.7578
Average Minimax Constrained Velocity Estimation Error = 12.6786
Average Kalman Unconstrained Position Estimation Error = 44.2402
Average Kalman Constrained Position Estimation Error (W=I) = 41.7381
Average Minimax Unconstrained Position Estimation Error = 32.0585
Average Minimax Constrained Position Estimation Error = 26.0899
Average Kalman Unconstrained Velocity Estimation Error = 11.6107
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.5352
Average Minimax Unconstrained Velocity Estimation Error = 13.2449
Average Minimax Constrained Velocity Estimation Error = 13.1458
Average Kalman Unconstrained Position Estimation Error = 50.2522
Average Kalman Constrained Position Estimation Error (W=I) = 47.5861
Average Minimax Unconstrained Position Estimation Error = 37.8228
Average Minimax Constrained Position Estimation Error = 31.9748
Average Kalman Unconstrained Velocity Estimation Error = 11.5924
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.5118
Average Minimax Unconstrained Velocity Estimation Error = 13.2507
Average Minimax Constrained Velocity Estimation Error = 13.1508
Average Kalman Unconstrained Position Estimation Error = 40.4
Average Kalman Constrained Position Estimation Error (W=I) = 38.4502
Average Minimax Unconstrained Position Estimation Error = 30.5488
Average Minimax Constrained Position Estimation Error = 25.3597
Average Kalman Unconstrained Velocity Estimation Error = 10.8566
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.7972
Average Minimax Unconstrained Velocity Estimation Error = 12.3851
Average Minimax Constrained Velocity Estimation Error = 12.2928
Average Kalman Unconstrained Position Estimation Error = 44.3675
Average Kalman Constrained Position Estimation Error (W=I) = 41.0058
Average Minimax Unconstrained Position Estimation Error = 34.051
Average Minimax Constrained Position Estimation Error = 26.8666
Average Kalman Unconstrained Velocity Estimation Error = 11.196
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.0627
Average Minimax Unconstrained Velocity Estimation Error = 12.7466
Average Minimax Constrained Velocity Estimation Error = 12.6064
Average Kalman Unconstrained Position Estimation Error = 45.6515
Average Kalman Constrained Position Estimation Error (W=I) = 43.2999
Average Minimax Unconstrained Position Estimation Error = 33.5739
Average Minimax Constrained Position Estimation Error = 27.9718
Average Kalman Unconstrained Velocity Estimation Error = 12.0421
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.9544
Average Minimax Unconstrained Velocity Estimation Error = 13.7248
Average Minimax Constrained Velocity Estimation Error = 13.6207
Average Kalman Unconstrained Position Estimation Error = 40.1945
Average Kalman Constrained Position Estimation Error (W=I) = 37.7049
Average Minimax Unconstrained Position Estimation Error = 30.8462
Average Minimax Constrained Position Estimation Error = 23.9413
Average Kalman Unconstrained Velocity Estimation Error = 11.9833
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.9072
Average Minimax Unconstrained Velocity Estimation Error = 13.6468
Average Minimax Constrained Velocity Estimation Error = 13.5379
Average Kalman Unconstrained Position Estimation Error = 47.2881
Average Kalman Constrained Position Estimation Error (W=I) = 44.3592
Average Minimax Unconstrained Position Estimation Error = 33.9691
Average Minimax Constrained Position Estimation Error = 28.3087
Average Kalman Unconstrained Velocity Estimation Error = 12.3173
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.2509
Average Minimax Unconstrained Velocity Estimation Error = 14.1279
Average Minimax Constrained Velocity Estimation Error = 14.0475
Average Kalman Unconstrained Position Estimation Error = 47.9738
Average Kalman Constrained Position Estimation Error (W=I) = 46.6696
Average Minimax Unconstrained Position Estimation Error = 34.5838
Average Minimax Constrained Position Estimation Error = 30.2156
Average Kalman Unconstrained Velocity Estimation Error = 12.1705
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.1165
Average Minimax Unconstrained Velocity Estimation Error = 13.9383
Average Minimax Constrained Velocity Estimation Error = 13.8472
Average Kalman Unconstrained Position Estimation Error = 44.687
Average Kalman Constrained Position Estimation Error (W=I) = 42.0819
Average Minimax Unconstrained Position Estimation Error = 33.57
Average Minimax Constrained Position Estimation Error = 27.618
Average Kalman Unconstrained Velocity Estimation Error = 11.0413
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.9547
Average Minimax Unconstrained Velocity Estimation Error = 12.5548
Average Minimax Constrained Velocity Estimation Error = 12.4475
Average Kalman Unconstrained Position Estimation Error = 51.9663
Average Kalman Constrained Position Estimation Error (W=I) = 49.198
Average Minimax Unconstrained Position Estimation Error = 39.3062
Average Minimax Constrained Position Estimation Error = 32.6792
Average Kalman Unconstrained Velocity Estimation Error = 12.9522
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.8529
Average Minimax Unconstrained Velocity Estimation Error = 14.7022
Average Minimax Constrained Velocity Estimation Error = 14.57
Average Kalman Unconstrained Position Estimation Error = 46.0978
Average Kalman Constrained Position Estimation Error (W=I) = 43.1734
Average Minimax Unconstrained Position Estimation Error = 35.2249
Average Minimax Constrained Position Estimation Error = 28.6092
Average Kalman Unconstrained Velocity Estimation Error = 10.8253
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.7459
Average Minimax Unconstrained Velocity Estimation Error = 12.4056
Average Minimax Constrained Velocity Estimation Error = 12.2949
Average Kalman Unconstrained Position Estimation Error = 50.1709
Average Kalman Constrained Position Estimation Error (W=I) = 46.4869
Average Minimax Unconstrained Position Estimation Error = 37.8666
Average Minimax Constrained Position Estimation Error = 31.1252
Average Kalman Unconstrained Velocity Estimation Error = 12.2813
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.1874
Average Minimax Unconstrained Velocity Estimation Error = 14.1079
Average Minimax Constrained Velocity Estimation Error = 13.9922
Average Kalman Unconstrained Position Estimation Error = 46.0197
Average Kalman Constrained Position Estimation Error (W=I) = 43.3882
Average Minimax Unconstrained Position Estimation Error = 35.4694
Average Minimax Constrained Position Estimation Error = 29.3232
Average Kalman Unconstrained Velocity Estimation Error = 11.9913
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.9043
Average Minimax Unconstrained Velocity Estimation Error = 13.6703
Average Minimax Constrained Velocity Estimation Error = 13.5381
Average Kalman Unconstrained Position Estimation Error = 42.3307
Average Kalman Constrained Position Estimation Error (W=I) = 39.7976
Average Minimax Unconstrained Position Estimation Error = 32.9069
Average Minimax Constrained Position Estimation Error = 27.0972
Average Kalman Unconstrained Velocity Estimation Error = 10.1768
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.089
Average Minimax Unconstrained Velocity Estimation Error = 11.6263
Average Minimax Constrained Velocity Estimation Error = 11.5083
Average Kalman Unconstrained Position Estimation Error = 47.1016
Average Kalman Constrained Position Estimation Error (W=I) = 44.4491
Average Minimax Unconstrained Position Estimation Error = 35.0421
Average Minimax Constrained Position Estimation Error = 28.6648
Average Kalman Unconstrained Velocity Estimation Error = 11.454
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.3731
Average Minimax Unconstrained Velocity Estimation Error = 13.2651
Average Minimax Constrained Velocity Estimation Error = 13.1522
Average Kalman Unconstrained Position Estimation Error = 53.8961
Average Kalman Constrained Position Estimation Error (W=I) = 52.3521
Average Minimax Unconstrained Position Estimation Error = 38.8041
Average Minimax Constrained Position Estimation Error = 33.573
Average Kalman Unconstrained Velocity Estimation Error = 13.4914
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.4299
Average Minimax Unconstrained Velocity Estimation Error = 15.5324
Average Minimax Constrained Velocity Estimation Error = 15.4454
Average Kalman Unconstrained Position Estimation Error = 42.814
Average Kalman Constrained Position Estimation Error (W=I) = 38.8566
Average Minimax Unconstrained Position Estimation Error = 32.4482
Average Minimax Constrained Position Estimation Error = 24.9523
Average Kalman Unconstrained Velocity Estimation Error = 11.4971
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.4231
Average Minimax Unconstrained Velocity Estimation Error = 13.1057
Average Minimax Constrained Velocity Estimation Error = 12.9972
Average Kalman Unconstrained Position Estimation Error = 39.6471
Average Kalman Constrained Position Estimation Error (W=I) = 36.6156
Average Minimax Unconstrained Position Estimation Error = 32.8363
Average Minimax Constrained Position Estimation Error = 25.2785
Average Kalman Unconstrained Velocity Estimation Error = 10.857
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.744
Average Minimax Unconstrained Velocity Estimation Error = 12.4976
Average Minimax Constrained Velocity Estimation Error = 12.3456
Average Kalman Unconstrained Position Estimation Error = 45.5402
Average Kalman Constrained Position Estimation Error (W=I) = 42.5557
Average Minimax Unconstrained Position Estimation Error = 33.4128
Average Minimax Constrained Position Estimation Error = 26.7385
Average Kalman Unconstrained Velocity Estimation Error = 11.6968
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.6071
Average Minimax Unconstrained Velocity Estimation Error = 13.5063
Average Minimax Constrained Velocity Estimation Error = 13.3969
Average Kalman Unconstrained Position Estimation Error = 50.738
Average Kalman Constrained Position Estimation Error (W=I) = 49.1928
Average Minimax Unconstrained Position Estimation Error = 36.6874
Average Minimax Constrained Position Estimation Error = 32.0083
Average Kalman Unconstrained Velocity Estimation Error = 13.1566
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.1044
Average Minimax Unconstrained Velocity Estimation Error = 15.0638
Average Minimax Constrained Velocity Estimation Error = 14.9871
Average Kalman Unconstrained Position Estimation Error = 43.9988
Average Kalman Constrained Position Estimation Error (W=I) = 41.6779
Average Minimax Unconstrained Position Estimation Error = 32.5733
Average Minimax Constrained Position Estimation Error = 26.2085
Average Kalman Unconstrained Velocity Estimation Error = 12.0699
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.9969
Average Minimax Unconstrained Velocity Estimation Error = 13.841
Average Minimax Constrained Velocity Estimation Error = 13.7378
Average Kalman Unconstrained Position Estimation Error = 43.2587
Average Kalman Constrained Position Estimation Error (W=I) = 41.3699
Average Minimax Unconstrained Position Estimation Error = 31.5188
Average Minimax Constrained Position Estimation Error = 26.1959
Average Kalman Unconstrained Velocity Estimation Error = 12.3453
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.2892
Average Minimax Unconstrained Velocity Estimation Error = 13.9913
Average Minimax Constrained Velocity Estimation Error = 13.9131
Average Kalman Unconstrained Position Estimation Error = 37.6277
Average Kalman Constrained Position Estimation Error (W=I) = 35.4972
Average Minimax Unconstrained Position Estimation Error = 28.5866
Average Minimax Constrained Position Estimation Error = 23.0281
Average Kalman Unconstrained Velocity Estimation Error = 10.4033
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.3378
Average Minimax Unconstrained Velocity Estimation Error = 11.8835
Average Minimax Constrained Velocity Estimation Error = 11.7782
Average Kalman Unconstrained Position Estimation Error = 44.9982
Average Kalman Constrained Position Estimation Error (W=I) = 42.2043
Average Minimax Unconstrained Position Estimation Error = 33.4348
Average Minimax Constrained Position Estimation Error = 26.932
Average Kalman Unconstrained Velocity Estimation Error = 12.0986
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.009
Average Minimax Unconstrained Velocity Estimation Error = 13.7595
Average Minimax Constrained Velocity Estimation Error = 13.6503
Average Kalman Unconstrained Position Estimation Error = 40.8362
Average Kalman Constrained Position Estimation Error (W=I) = 36.9589
Average Minimax Unconstrained Position Estimation Error = 31.9791
Average Minimax Constrained Position Estimation Error = 24.3034
Average Kalman Unconstrained Velocity Estimation Error = 10.7195
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.6002
Average Minimax Unconstrained Velocity Estimation Error = 12.1433
Average Minimax Constrained Velocity Estimation Error = 12.0114
Average Kalman Unconstrained Position Estimation Error = 44.898
Average Kalman Constrained Position Estimation Error (W=I) = 42.9832
Average Minimax Unconstrained Position Estimation Error = 33.4475
Average Minimax Constrained Position Estimation Error = 27.8622
Average Kalman Unconstrained Velocity Estimation Error = 11.7762
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.7129
Average Minimax Unconstrained Velocity Estimation Error = 13.4307
Average Minimax Constrained Velocity Estimation Error = 13.3252
Average Kalman Unconstrained Position Estimation Error = 43.737
Average Kalman Constrained Position Estimation Error (W=I) = 39.998
Average Minimax Unconstrained Position Estimation Error = 34.7594
Average Minimax Constrained Position Estimation Error = 26.6063
Average Kalman Unconstrained Velocity Estimation Error = 10.3466
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.2259
Average Minimax Unconstrained Velocity Estimation Error = 11.832
Average Minimax Constrained Velocity Estimation Error = 11.6668
Average Kalman Unconstrained Position Estimation Error = 46.3306
Average Kalman Constrained Position Estimation Error (W=I) = 44.3844
Average Minimax Unconstrained Position Estimation Error = 34.6959
Average Minimax Constrained Position Estimation Error = 29.8334
Average Kalman Unconstrained Velocity Estimation Error = 11.4752
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.404
Average Minimax Unconstrained Velocity Estimation Error = 13.134
Average Minimax Constrained Velocity Estimation Error = 13.034
Average Kalman Unconstrained Position Estimation Error = 45.1653
Average Kalman Constrained Position Estimation Error (W=I) = 41.3958
Average Minimax Unconstrained Position Estimation Error = 34.7535
Average Minimax Constrained Position Estimation Error = 26.9856
Average Kalman Unconstrained Velocity Estimation Error = 11.6046
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.4985
Average Minimax Unconstrained Velocity Estimation Error = 13.411
Average Minimax Constrained Velocity Estimation Error = 13.2696
Average Kalman Unconstrained Position Estimation Error = 44.8207
Average Kalman Constrained Position Estimation Error (W=I) = 41.6311
Average Minimax Unconstrained Position Estimation Error = 33.9562
Average Minimax Constrained Position Estimation Error = 27.1428
Average Kalman Unconstrained Velocity Estimation Error = 11.5605
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.4491
Average Minimax Unconstrained Velocity Estimation Error = 13.2212
Average Minimax Constrained Velocity Estimation Error = 13.0972
Average Kalman Unconstrained Position Estimation Error = 46.4874
Average Kalman Constrained Position Estimation Error (W=I) = 43.1619
Average Minimax Unconstrained Position Estimation Error = 35.8105
Average Minimax Constrained Position Estimation Error = 28.2794
Average Kalman Unconstrained Velocity Estimation Error = 11.268
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.1205
Average Minimax Unconstrained Velocity Estimation Error = 12.8349
Average Minimax Constrained Velocity Estimation Error = 12.663
Average Kalman Unconstrained Position Estimation Error = 46.7708
Average Kalman Constrained Position Estimation Error (W=I) = 44.8111
Average Minimax Unconstrained Position Estimation Error = 33.5135
Average Minimax Constrained Position Estimation Error = 28.4436
Average Kalman Unconstrained Velocity Estimation Error = 12.3379
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.279
Average Minimax Unconstrained Velocity Estimation Error = 14.1676
Average Minimax Constrained Velocity Estimation Error = 14.0868
Average Kalman Unconstrained Position Estimation Error = 48.0693
Average Kalman Constrained Position Estimation Error (W=I) = 45.6457
Average Minimax Unconstrained Position Estimation Error = 35.5854
Average Minimax Constrained Position Estimation Error = 30.0731
Average Kalman Unconstrained Velocity Estimation Error = 11.8151
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.7323
Average Minimax Unconstrained Velocity Estimation Error = 13.6136
Average Minimax Constrained Velocity Estimation Error = 13.5136
Average Kalman Unconstrained Position Estimation Error = 49.1042
Average Kalman Constrained Position Estimation Error (W=I) = 47.4221
Average Minimax Unconstrained Position Estimation Error = 36.6487
Average Minimax Constrained Position Estimation Error = 31.6903
Average Kalman Unconstrained Velocity Estimation Error = 12.3584
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.2932
Average Minimax Unconstrained Velocity Estimation Error = 14.0965
Average Minimax Constrained Velocity Estimation Error = 14.0048
Average Kalman Unconstrained Position Estimation Error = 47.7918
Average Kalman Constrained Position Estimation Error (W=I) = 45.3963
Average Minimax Unconstrained Position Estimation Error = 35.0344
Average Minimax Constrained Position Estimation Error = 29.0528
Average Kalman Unconstrained Velocity Estimation Error = 12.5663
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.4868
Average Minimax Unconstrained Velocity Estimation Error = 14.4807
Average Minimax Constrained Velocity Estimation Error = 14.3774
Average Kalman Unconstrained Position Estimation Error = 48.7593
Average Kalman Constrained Position Estimation Error (W=I) = 46.4848
Average Minimax Unconstrained Position Estimation Error = 36.6262
Average Minimax Constrained Position Estimation Error = 30.4388
Average Kalman Unconstrained Velocity Estimation Error = 12.5956
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.5088
Average Minimax Unconstrained Velocity Estimation Error = 14.4646
Average Minimax Constrained Velocity Estimation Error = 14.3454
Average Kalman Unconstrained Position Estimation Error = 49.66
Average Kalman Constrained Position Estimation Error (W=I) = 47.1749
Average Minimax Unconstrained Position Estimation Error = 36.9599
Average Minimax Constrained Position Estimation Error = 31.4104
Average Kalman Unconstrained Velocity Estimation Error = 12.1661
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.0803
Average Minimax Unconstrained Velocity Estimation Error = 13.9418
Average Minimax Constrained Velocity Estimation Error = 13.83
Average Kalman Unconstrained Position Estimation Error = 48.1765
Average Kalman Constrained Position Estimation Error (W=I) = 45.7791
Average Minimax Unconstrained Position Estimation Error = 35.0313
Average Minimax Constrained Position Estimation Error = 29.49
Average Kalman Unconstrained Velocity Estimation Error = 12.445
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.3884
Average Minimax Unconstrained Velocity Estimation Error = 14.2258
Average Minimax Constrained Velocity Estimation Error = 14.1327
Average Kalman Unconstrained Position Estimation Error = 48.9413
Average Kalman Constrained Position Estimation Error (W=I) = 45.6302
Average Minimax Unconstrained Position Estimation Error = 35.1817
Average Minimax Constrained Position Estimation Error = 28.4907
Average Kalman Unconstrained Velocity Estimation Error = 12.7262
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.6163
Average Minimax Unconstrained Velocity Estimation Error = 14.5619
Average Minimax Constrained Velocity Estimation Error = 14.4525
Average Kalman Unconstrained Position Estimation Error = 46.5021
Average Kalman Constrained Position Estimation Error (W=I) = 43.8823
Average Minimax Unconstrained Position Estimation Error = 34.1878
Average Minimax Constrained Position Estimation Error = 28.8433
Average Kalman Unconstrained Velocity Estimation Error = 11.4334
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.3613
Average Minimax Unconstrained Velocity Estimation Error = 13.1192
Average Minimax Constrained Velocity Estimation Error = 13.0327
Average Kalman Unconstrained Position Estimation Error = 51.2867
Average Kalman Constrained Position Estimation Error (W=I) = 49.9151
Average Minimax Unconstrained Position Estimation Error = 35.8106
Average Minimax Constrained Position Estimation Error = 31.3526
Average Kalman Unconstrained Velocity Estimation Error = 13.062
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.0065
Average Minimax Unconstrained Velocity Estimation Error = 15.0109
Average Minimax Constrained Velocity Estimation Error = 14.9273
Average Kalman Unconstrained Position Estimation Error = 50.842
Average Kalman Constrained Position Estimation Error (W=I) = 48.8161
Average Minimax Unconstrained Position Estimation Error = 35.2774
Average Minimax Constrained Position Estimation Error = 30.1152
Average Kalman Unconstrained Velocity Estimation Error = 13.476
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.4142
Average Minimax Unconstrained Velocity Estimation Error = 15.2906
Average Minimax Constrained Velocity Estimation Error = 15.2115
Average Kalman Unconstrained Position Estimation Error = 50.9405
Average Kalman Constrained Position Estimation Error (W=I) = 48.8866
Average Minimax Unconstrained Position Estimation Error = 36.0227
Average Minimax Constrained Position Estimation Error = 31.2364
Average Kalman Unconstrained Velocity Estimation Error = 13.0744
Average Kalman Constrained Velocity Estimation Error (W=I) = 13.0013
Average Minimax Unconstrained Velocity Estimation Error = 14.9296
Average Minimax Constrained Velocity Estimation Error = 14.8415
Average Kalman Unconstrained Position Estimation Error = 40.9326
Average Kalman Constrained Position Estimation Error (W=I) = 37.4228
Average Minimax Unconstrained Position Estimation Error = 32.4619
Average Minimax Constrained Position Estimation Error = 24.3225
Average Kalman Unconstrained Velocity Estimation Error = 10.1547
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.0387
Average Minimax Unconstrained Velocity Estimation Error = 11.5519
Average Minimax Constrained Velocity Estimation Error = 11.3949
Average Kalman Unconstrained Position Estimation Error = 46.1606
Average Kalman Constrained Position Estimation Error (W=I) = 44.4093
Average Minimax Unconstrained Position Estimation Error = 34.2305
Average Minimax Constrained Position Estimation Error = 29.8654
Average Kalman Unconstrained Velocity Estimation Error = 11.6564
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.606
Average Minimax Unconstrained Velocity Estimation Error = 13.3809
Average Minimax Constrained Velocity Estimation Error = 13.305
Average Kalman Unconstrained Position Estimation Error = 41.4939
Average Kalman Constrained Position Estimation Error (W=I) = 38.2955
Average Minimax Unconstrained Position Estimation Error = 30.8223
Average Minimax Constrained Position Estimation Error = 23.7801
Average Kalman Unconstrained Velocity Estimation Error = 11.1491
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.0035
Average Minimax Unconstrained Velocity Estimation Error = 12.7594
Average Minimax Constrained Velocity Estimation Error = 12.618
Average Kalman Unconstrained Position Estimation Error = 50.2061
Average Kalman Constrained Position Estimation Error (W=I) = 48.5323
Average Minimax Unconstrained Position Estimation Error = 37.389
Average Minimax Constrained Position Estimation Error = 33.2764
Average Kalman Unconstrained Velocity Estimation Error = 11.8434
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.7803
Average Minimax Unconstrained Velocity Estimation Error = 13.6677
Average Minimax Constrained Velocity Estimation Error = 13.5807
Average Kalman Unconstrained Position Estimation Error = 43.0854
Average Kalman Constrained Position Estimation Error (W=I) = 40.9222
Average Minimax Unconstrained Position Estimation Error = 32.004
Average Minimax Constrained Position Estimation Error = 26.577
Average Kalman Unconstrained Velocity Estimation Error = 11.8591
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.8116
Average Minimax Unconstrained Velocity Estimation Error = 13.6318
Average Minimax Constrained Velocity Estimation Error = 13.5489
Average Kalman Unconstrained Position Estimation Error = 43.4093
Average Kalman Constrained Position Estimation Error (W=I) = 41.3344
Average Minimax Unconstrained Position Estimation Error = 32.8742
Average Minimax Constrained Position Estimation Error = 28.3643
Average Kalman Unconstrained Velocity Estimation Error = 11.3373
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.2667
Average Minimax Unconstrained Velocity Estimation Error = 13.004
Average Minimax Constrained Velocity Estimation Error = 12.9116
Average Kalman Unconstrained Position Estimation Error = 46.1742
Average Kalman Constrained Position Estimation Error (W=I) = 43.8565
Average Minimax Unconstrained Position Estimation Error = 33.8145
Average Minimax Constrained Position Estimation Error = 28.0379
Average Kalman Unconstrained Velocity Estimation Error = 10.9616
Average Kalman Constrained Velocity Estimation Error (W=I) = 10.871
Average Minimax Unconstrained Velocity Estimation Error = 12.5539
Average Minimax Constrained Velocity Estimation Error = 12.4452
Average Kalman Unconstrained Position Estimation Error = 49.1778
Average Kalman Constrained Position Estimation Error (W=I) = 46.4639
Average Minimax Unconstrained Position Estimation Error = 37.068
Average Minimax Constrained Position Estimation Error = 30.4369
Average Kalman Unconstrained Velocity Estimation Error = 12.4239
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.3167
Average Minimax Unconstrained Velocity Estimation Error = 14.1242
Average Minimax Constrained Velocity Estimation Error = 13.9904
Average Kalman Unconstrained Position Estimation Error = 51.2107
Average Kalman Constrained Position Estimation Error (W=I) = 48.7932
Average Minimax Unconstrained Position Estimation Error = 38.0309
Average Minimax Constrained Position Estimation Error = 32.3017
Average Kalman Unconstrained Velocity Estimation Error = 12.4833
Average Kalman Constrained Velocity Estimation Error (W=I) = 12.4189
Average Minimax Unconstrained Velocity Estimation Error = 14.3205
Average Minimax Constrained Velocity Estimation Error = 14.2334
Average Kalman Unconstrained Position Estimation Error = 44.9408
Average Kalman Constrained Position Estimation Error (W=I) = 42.1332
Average Minimax Unconstrained Position Estimation Error = 33.7469
Average Minimax Constrained Position Estimation Error = 27.3958
Average Kalman Unconstrained Velocity Estimation Error = 11.925
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.8499
Average Minimax Unconstrained Velocity Estimation Error = 13.6614
Average Minimax Constrained Velocity Estimation Error = 13.5632
Average Kalman Unconstrained Position Estimation Error = 48.277
Average Kalman Constrained Position Estimation Error (W=I) = 47.1215
Average Minimax Unconstrained Position Estimation Error = 35.3489
Average Minimax Constrained Position Estimation Error = 31.7142
Average Kalman Unconstrained Velocity Estimation Error = 11.9219
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.883
Average Minimax Unconstrained Velocity Estimation Error = 13.6831
Average Minimax Constrained Velocity Estimation Error = 13.6172
Average Kalman Unconstrained Position Estimation Error = 46.5796
Average Kalman Constrained Position Estimation Error (W=I) = 44.311
Average Minimax Unconstrained Position Estimation Error = 34.785
Average Minimax Constrained Position Estimation Error = 29.6329
Average Kalman Unconstrained Velocity Estimation Error = 11.8273
Average Kalman Constrained Velocity Estimation Error (W=I) = 11.7569
Average Minimax Unconstrained Velocity Estimation Error = 13.5106
Average Minimax Constrained Velocity Estimation Error = 13.4197
average RMS position estimation errors:
48.7802, unconstrained Kalman filter
46.7093, constrained Kalman filter
37.8502, unconstrained H_\infty filter
33.0811, constrained H_\infty filter
average RMS velocity estimation errors:
12.268, unconstrained Kalman filter
12.1986, constrained Kalman filter
13.9733, unconstrained H_\infty filter
13.8782, constrained H_\infty filter
5. 小结
运动控制在现代生活中的实际应用非常广泛,除了智能工厂中各种智能设备的自动运转控制,近几年最火的自动驾驶技术,以及航空航天领域,都缺少不了它的身影,所以熟练掌握状态估计理论,对未来就业也是非常有帮助的。切记矩阵理论与概率论等知识的基础一定要打好。对本章内容感兴趣或者想充分学习了解的,建议去研习书中第十二章节的内容,有条件的可以通过习题的联系进一步巩固充实。后期会对其中一些知识点在自己理解的基础上进行讨论补充,欢迎大家一起学习交流。
原书链接:Optimal State Estimation:Kalman, H-infinity, and Nonlinear Approaches