《最优状态估计-卡尔曼，H∞及非线性滤波》：第9章 最优平滑

前言

《最优状态估计-卡尔曼，H∞及非线性滤波》由国外引进的一本关于状态估计的专业书籍，2006年正式出版，作者是Dan Simon教授，来自克利夫兰州立大学，电气与计算机工程系。主要应用于运动估计与控制，学习本文的过程中需要有一定的专业基础知识打底。

本书共分为四个部分，全面介绍了最优状态估计的理论和方法。第1部分为基础知识，回顾了线性系统、概率论和随机过程相关知识，介绍了最小二乘法、维纳滤波、状态的统计特性随时间的传播过程。第2部分详细介绍了卡尔曼滤波及其等价形式，介绍了卡尔曼滤波的扩展形式，包括相关噪声和有色噪声条件下的卡尔曼滤波、稳态滤波、衰减记忆滤波和带约束的卡尔曼滤波等（掌握了卡尔曼，基本上可以说这本书掌握了一半）。第3部分详细介绍了H∞滤波，包括时域和频域的H∞滤波，混合卡尔曼/H∞滤波，带约束的H∞ 滤波。第4部分介绍非线性系统滤波方法，包括扩展卡尔曼滤波、无迹卡尔曼滤波及粒子滤波。本书适合作为最优状态估计相关课程的高年级本科生或研究生教材，或从事相关研究工作人员的参考书。

其实自己研究生期间的主研方向并不是运动控制，但自己在本科大三时参加过智能车大赛，当时是采用PID对智能车的运动进行控制，彼时凭借着自学的一知半解，侥幸拿到了奖项。到了研究生期间，实验室正好有研究平衡车的项目，虽然自己不是那个方向，但实验室经常有组内报告，所以对运动控制在实际项目中的应用也算有了基本的了解。参加工作后，有需要对运动估计与控制进行相关研究，所以接触到这本书。

这次重新捡起运动控制，是希望自己可以将这方面的知识进行巩固再学习，结合原书的习题例程进行仿真，简单记录一下这个过程。主要以各章节中习题仿真为主，这是本书的第九章的3个仿真示例（仿真平台：32位MATLAB2015b），话不多说，开始！

1. MATLAB仿真：示例9.1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例9.1: FixPtSmooth.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xsmooth] = FixPtSmooth(duration, dt, measnoise, PlotFlag)

% function FixPtSmooth(duration, dt)
%
% Fixed point smoother simulation for a two state navigation problem.
% INPUTS
%   duration = length of simulation (seconds)
%   dt = step size (seconds)
%   measnoise = standard deviation of position measurement noise
%   PlotFlag = true/false flag saying whether or not to generate plots
% OUTPUT
%   xsmooth = estimate of the initial state after all measurements have been processed

if ~exist('duration', 'var')
    duration = 10;
end
if ~exist('dt', 'var')
    dt = 0.1;
end
if ~exist('measnoise', 'var')
    measnoise = 1; 
end
if ~exist('PlotFlag', 'var')
    PlotFlag = true;
end

accelnoise = 0.2; % acceleration noise (feet/sec^2)

a = [1 dt; 0 1]; % transition matrix
b = [dt^2/2; dt]; % input matrix
c = [1 0]; % measurement matrix

Sw = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
P = [1 0; 0 1]; % initial the a posteriori estimation covariance
Sz = measnoise^2; % measurement error covariance

x = [0; 0]; % initial state vector
xhat = x + sqrt(P) * 2 * ones(size(x)); % initial the a posteriori state estimate (in error by 2 sigma)

u = 10; % Use a constant commanded acceleration

xhat = a * xhat + b * u; % initial the a priori state estimate
P = a * P * a' + Sw; % initial the a priori covariance

% Initialize the fixed point smoother
Sigma = P;
Pi = P;
xsmooth = xhat;

% Initialize arrays for later plotting.
pos = []; % true position array
poshat = []; % estimated position array
possmooth = []; % smoothed position array
posmeas = []; % measured position array
vel = []; % true velocity array
velhat = []; % estimated velocity array
velsmooth = []; % smoothed velocity array
TrPArray = []; % Trace of standard estimation covariance
TrPiArray = []; % Trace of smoothed estimation covariance

randn('state', sum(100*clock)); % initialize the random number generator

% Simulate the linear system and the noisy measurement.
ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
x = a * x + b * u + ProcessNoise;
y = c * x + measnoise * randn;
% Form the Innovation vector.
Inn = y - c * xhat;

for t = 0 : dt: duration
    % Save some parameters for plotting later.
    pos = [pos; x(1)];
    posmeas = [posmeas; y];
    poshat = [poshat; xhat(1)];
    possmooth = [possmooth; xsmooth(1)];
    vel = [vel; x(2)];
    velhat = [velhat; xhat(2)];
    velsmooth = [velsmooth; xsmooth(2)];
    TrPArray = [TrPArray trace(P)];
    TrPiArray = [TrPiArray trace(Pi)];
    
    % Compute the covariance of the Innovation.
    s = c * P * c' + Sz;
    % Form the Kalman Gain matrix.
    K = a * P * c' * inv(s);
    
    % Compute the smoothed estimate.
    Lambda = Sigma * c' * inv(s); % smoother gain
    Pi = Pi - Sigma * c' * Lambda'; % covariance of smoothed estimate
    Sigma = Sigma * (a - K * c)'; % cross covariance of standard and smoothed estimates
    xsmooth = xsmooth + Lambda * Inn; % smoothed estimate of the initial state
    
    % Update the state estimate.
    xhat = a * xhat + K * Inn + b * u;
    % Compute the covariance of the estimation error.
    P = a * P * a' - a * P * c' * K' + Sw;
    
    % Simulate the linear system and the noisy measurement.
    ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
    x = a * x + b * u + ProcessNoise;
    y = c * x + measnoise * randn;
    % Form the Innovation vector.
    Inn = y - c * xhat;
    
end

if ~PlotFlag
    return;
end

disp(['Pi(1,1) = ', num2str(Pi(1,1)), ', Pi(2,2) = ', num2str(Pi(2,2))]);
Improve = 100 * (TrPArray(1) - trace(Pi)) / TrPArray(1);
disp(['Smoothing Improvement = ',num2str(Improve),' %']);

% Plot the results
close all;
t = 0 : dt : duration;

figure;
plot(t,TrPArray,'r-', t,TrPiArray,'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Analytical Estimation Covariance', 'FontSize', 12);
legend('Standard Filter', 'Smoothed Filter'); 

figure;
plot(t,possmooth-pos(1),'r', t,velsmooth-vel(1),'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Smoothing Error at Initial Time', 'FontSize', 12);
legend('Position Error', 'Velocity Error'); 

figure;
plot(t,pos,'r-', t,posmeas,'b:', t,poshat,'k--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Position');
legend('True Position', 'Measured Position', 'Estimated Position');

figure;
plot(t,pos-posmeas,'r-', t,pos-poshat,'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time (sec)');
ylabel('Position Error');
legend('Position Measurement Error', 'Position Estimation Error');

figure;
plot(t,vel,'r-', t,velhat,'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Velocity');
legend('True Velocity', 'Estimated Velocity');

figure;
plot(t,vel-velhat);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Velocity Estimation Error');

>> FixPtSmooth
Pi(1,1) = 0.057377, Pi(2,2) = 0.011954
Smoothing Improvement = 96.5514 %

ans =

    0.5553
    0.7999

2. MATLAB仿真：示例9.2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例9.2: FixLagSmooth.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [PctImprovement] = FixLagSmooth(duration, dt, N, measnoise)

% function FixLagSmooth(duration, dt)
% Updated September 2016
%
% Fixed lag smoother simulation for a vehicle traveling along a road.
% INPUTS
%   duration = length of simulation (seconds)
%   dt = step size (seconds)
%   N = one less than smoothing lag
%   measnoise
% OUTPUTS
%   PctImprovement = array containing the percent improvement due to
%                    smoothing with a lag of 1, 2, ..., N+1

accelnoise = 10; % acceleration noise (feet/sec^2)

if ~exist('duration', 'var')
    duration = 10;
end
if ~exist('dt', 'var')
    dt = 0.1;
end
if ~exist('N', 'var')
    N = 30;
end
if ~exist('measnoise', 'var')
    measnoise = 10; % position measurement noise (feet)
end

a = [1 dt; 0 1]; % transition matrix
b = [dt^2/2; dt]; % input matrix
c = [1 0]; % measurement matrix
x = [0; 0]; % initial state vector
xhat = x; % initial state estimate

Sw = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
P = [1 0; 0 1]; % initial estimation covariance
Sz = measnoise^2; % measurement error covariance

PCol = zeros(2,2,N+1);
PColOld = zeros(2,2,N+1);
PSmooth = zeros(2,2,N+1);
PSmoothOld = zeros(2,2,N+1);

% Initialize arrays for later plotting.
pos = []; % true position array
poshat = []; % estimated position array
posmeas = []; % measured position array
vel = []; % true velocity array
velhat = []; % estimated velocity array

kk=0;
for t = 0 : dt: duration,
    kk=kk+1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%TIME INDEX
    % Use a constant commanded acceleration of 1 foot/sec^2.
    u = 1;
    % Simulate the linear system.
    ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
    x = a * x + b * u + ProcessNoise;
    % Simulate the noisy measurement
    MeasNoise = measnoise * randn;
    y = c * x + MeasNoise;
    % Extrapolate the most recent state estimate to the present time.
    xhat = a * xhat + b * u;
    % Smoothed estimated at time (kk) given measurements up to and including time (kk-1) - this is the same as the prior xhat calculated on the previous line
    xhatSmoothArr(:,kk+1,kk) = xhat; 
    % Form the Innovation vector.
    Inn = y - c * xhat;
    % Compute the covariance of the Innovation.
    s = c * P * c' + Sz;
    % Form the Kalman Gain matrix.
    K = a * P * c' / s;
    KSmooth = K;
    % Update the state estimate.
    xhat = xhat + K * Inn;
    
    % Compute the covariance of the estimation error.
    PColOld(:,:,1) = P;
    PSmoothOld(:,:,1) = P;
    P = a * P * a' - a * P * c' / s * c * P * a' + Sw;
    % Save some parameters for plotting later.
    pos = [pos; x(1)];
    posmeas = [posmeas; y];
    poshat = [poshat; xhat(1)];
    vel = [vel; x(2)];
    velhat = [velhat; xhat(2)];
    for i = 1 : N+1
        KSmooth = PColOld(:,:,i) * c' / s;
        PSmooth(:,:,i+1) = PSmoothOld(:,:,i) - PColOld(:,:,i) * c' * KSmooth' * a';
        PCol(:,:,i+1) = PColOld(:,:,i) * (a - K * c)';
        if kk < i, break, end
        % Calculate smoothed estimated at time (kk+1-i) given measurements up to and including time kk
        xhatSmoothArr(:,kk+2-i,kk+1) = xhatSmoothArr(:,kk+2-i,kk) + KSmooth * Inn;
    end
    PSmoothOld = PSmooth;
    PColOld = PCol;
    
end

for i = 1 : N
    PctImprovement(i) = 100 * trace(P - PSmooth(:,:,i+1)) / trace(P);
end

close all
figure
plot(PctImprovement);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Number of lag intervals'); ylabel('Percent improvement');
title(['Measurement Noise = ',num2str(measnoise)], 'FontSize', 12);

figure
hold on
plot(0:kk-1, poshat,'b-')
plot(0:kk-1, posmeas,'r.')

Temp = NaN(kk, 1);
for i = 1 : kk-1
    Temp(i) = xhatSmoothArr(1, i, i+1);
end
plot(0:kk-1, Temp, 'k-')    % Smoothed first state with a lag of 1 time step

Temp = NaN(kk, 1);
for i = 1 : kk-5
    Temp(i) = xhatSmoothArr(1, i, i+5);
end
plot(0:kk-1, Temp, 'g')     % Smoothed first state with a lag of 5 time steps

Temp = NaN(kk, 1);
for i = 1 : kk-30
    Temp(i) = xhatSmoothArr(1, i, i+30);
end
plot(0:kk-1, Temp, 'm--')   % Smoothed first state with a lag of 30 time steps

set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time step')
legend('Posteriori estimation (time lag = 0)', 'Measurement', 'Smoothed estimate (time lag = 1)', 'Smoothed estimate (time lag = 5)', 'Smoothed estimate (time lag = 30)')
hold off

>> FixLagSmooth

ans =

  1 至 11 列

   10.5367   20.0939   28.6855   36.3418   43.1062   49.0319   54.1792   58.6126   62.3988   65.6046   68.2955

  12 至 22 列

   70.5340   72.3794   73.8865   75.1054   76.0814   76.8550   77.4617   77.9324   78.2937   78.5683   78.7751

  23 至 30 列

   78.9298   79.0453   79.1319   79.1978   79.2492   79.2912   79.3271   79.3596

3. MATLAB仿真：示例9.3

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例9.3: FixIntSmooth.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [Errf, Errb, Errs] = FixIntSmoothA(duration, dt, measnoise)

% function FixIntSmooth(duration, dt)
% 
% Fixed lag smoother simulation for a vehicle traveling along a road.
% Modified June 5, 2015 to correct Ibminus equation (line 97) and to add state estimate plots
%
% INPUTS
%   duration = length of simulation (seconds)
%   dt = step size (seconds)
%   measnoise = standard deviation of measurement noise
% OUTPUTS
%   Errf = norm of state estimation error of the forward filter at the desired time of estimation
%   Errb = norm of state estimation error of the backward filter at the desired time of estimation
%   Errf = norm of state estimation error of the smoothed filter at the desired time of estimation

accelnoise = 10; % acceleration noise (feet/sec^2)

if ~exist('duration', 'var')
   duration = 10;
end
if ~exist('dt', 'var')
   dt = 0.1;
end
if ~exist('measnoise', 'var')
	measnoise = 10; % position measurement noise (feet)
end

% We want to obtain a smoothed estimate at the halfway point.
EstPoint = duration / 2;

a = [1 dt; 0 1]; % transition matrix
b = [dt^2/2; dt]; % input matrix
c = [1 0]; % measurement matrix
x0 = [0; 0]; % initial state vector

Sw = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
Sz = measnoise^2; % measurement error covariance

xArr = [];
yArr = [];
% Simulate the system and collect the measurements.
% Use a constant commanded acceleration.
u = 10;
x = x0;
for t = 0 : dt: duration
    if t == EstPoint
        xTrue = x;
    end
    ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
    x = a * x + b * u + ProcessNoise;
    y = c * x + measnoise * randn;
    xArr = [xArr x];
    yArr = [yArr y];
end

% Obtain the forward estimate.
Pf = [20 0; 0 20]; % initial forward estimation covariance
PfArr = [];
xhatf = x0 + sqrt(Pf) * ones(size(x)); % initial state estimate
xhatfArr = [];
index = 1;
for t = 0 : dt : EstPoint
   % Extrapolate the most recent state estimate to the present time.
   xhatf = a * xhatf + b * u;
   % Form the Innovation vector.
   Inn = yArr(index) - c * xhatf;
   % Compute the covariance of the Innovation.
   CovInn = c * Pf * c' + Sz;
   % Compute the covariance of the estimation error.
   Pf = a * Pf * a' - a * Pf * c' / CovInn * c * Pf * a' + Sw;
   % Form the Kalman Gain matrix.
   K = a * Pf * c' / CovInn;
   % Update the state estimate.
   xhatf = xhatf + K * Inn;
   % Save some parameters for plotting later.
   PfArr = [PfArr trace(Pf)];
   xhatfArr = [xhatfArr xhatf];
   % Increment the pointer to the measurement array.
   index = index + 1;
end
Errf = norm(xTrue - xhatf);

% Obtain the backward estimate.
% The initial backward information matrix Ibminus needs to be set to a
% small nonzero matrix so that the first calculated Ibplus is invertible.
Ibminus = [0 0; 0 1e-12]; 
PbArr = [];
xhatbArr = [];
s = zeros(size(x)); % initial backward modified estimate
index = length(yArr);
Sw = Sw + [0 0; 0 0]; 
for t = duration : -dt : EstPoint + dt
    Ibplus = Ibminus + c' / Sz * c;
    s = s + c' / Sz * yArr(index);
    % The following line does not work unless Sw is invertible.
    %Ibminus = inv(Sw) - inv(Sw) * inv(a) * inv(Ibplus + inv(a)' * inv(Sw) * inv(a)) * inv(a)' * inv(Sw);
    Ibminus = inv(a \ inv(Ibplus) / a' + a \ Sw / a');
    s = Ibminus / a / Ibplus * s - Ibminus  / a * b * u;
    % Save some parameters for plotting later.
    Pbminus = inv(Ibminus);
    PbArr = [PbArr trace(Pbminus)];
    xhatbArr = [xhatbArr Ibminus\s];
    % Decrement the pointer to the measurement array.
    index = index - 1;
end
xhatb = Ibminus \ s;
Errb = norm(xTrue - xhatb);

% Obtain the smoothed estimate
Kf = Pbminus / (Pf + Pbminus);
xhat = Kf * xhatf + (eye(2) - Kf) * xhatb;
Errs = norm(xTrue - xhat);

disp(['forward error = ', num2str(Errf), ', backward error = ', num2str(Errb), ', smoothed error = ', num2str(Errs)]);

% Compute the smoothed estimation covariance.
P = inv(inv(Pf) + Ibminus);

close all;
set(gca,'FontSize',12); set(gcf,'Color','White');
plot(0:dt:EstPoint, PfArr, 'b', duration:-dt:EstPoint+dt, PbArr, 'r', EstPoint, trace(P), 'ko');
hold on;
plot(EstPoint, PfArr(end), 'bo', EstPoint+dt, PbArr(end), 'ro');
axis([0 duration 0 2*max(PfArr)]);
xlabel('Time step'); ylabel('Trace of estimation covariance');

figure
plot(0:dt:duration,xArr(1,:),'b', 0:dt:EstPoint,xhatfArr(1,:),'r', duration:-dt:EstPoint+dt,xhatbArr(1,:),'k');
xlabel('Time step'); ylabel('First State');
legend('True', 'Forward Estimate', 'Backward Estimate')

figure
plot(0:dt:duration,xArr(2,:),'b', 0:dt:EstPoint,xhatfArr(2,:),'r', duration:-dt:EstPoint+dt,xhatbArr(2,:),'k');
xlabel('Time step'); ylabel('Second State');
legend('True', 'Forward Estimate', 'Backward Estimate')

>> FixIntSmooth
forward error = 5.5212, backward error = 4.9749, smoothed error = 2.6704

ans =

    5.5212

4. 小结

运动控制在现代生活中的实际应用非常广泛，除了智能工厂中各种智能设备的自动运转控制，近几年最火的自动驾驶技术，以及航空航天领域，都缺少不了它的身影，所以熟练掌握状态估计理论，对未来就业也是非常有帮助的。切记矩阵理论与概率论等知识的基础一定要打好。对本章内容感兴趣或者想充分学习了解的，建议去研习书中第九章节的内容，有条件的可以通过习题的联系进一步巩固充实。后期会对其中一些知识点在自己理解的基础上进行讨论补充，欢迎大家一起学习交流。

原书链接：Optimal State Estimation:Kalman, H-infinity, and Nonlinear Approaches