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

• 前言
• 1. MATLAB仿真：示例5.1
• 2. MATLAB仿真：示例5.2
• 3. MATLAB仿真：示例5.3（1）
• 4. MATLAB仿真：示例5.3（2）
• 5. 小结

前言

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

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

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

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

1. MATLAB仿真：示例5.1

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

function DiscreteKFEx1(N)

% Discrete time Kalman filter for position estimation of a Newtonian system.
% This example illustrates the effectiveness of the Kalman filter for state
% estimation. It also shows how the variance of the estimation error 
% propagates between time steps and decreases as each measurement is processed.
% INPUTS: N = number of time steps.

if ~exist('N', 'var')
    N = 6;
end

T = 5; % time between measurements
sigma = 30; % position measurement standard deviation
R = sigma^2;
P0 = [100 0 0; 0 10 0; 0 0 1]; % initial state estimate uncertainty
A = [0 1 0; 0 0 1; 0 0 0];
H = [1 0 0];
F = [1 T T*T/2; 0 1 T; 0 0 1]; % state transition matrix
x = [1; 1; 1]; % initial state
xhat = x; % initial state estimate

posArray = [];
xhatArray = [];
yArray = [];
Pplus = P0;
Varminus = [];
Varplus = [P0(1,1)];
KArray = [];

for k = 1 : N
    % Simulate the system and measurement
    x = F * x;
    y = H * x + sigma * randn;
    % Estimate the state
    Pminus = F * Pplus * F';
    K = Pminus * H' * inv(H * Pminus * H' + R);
    xhat = F * xhat;
    xhat = xhat + K * (y - H * xhat);
    Pplus = (eye(3) - K * H) * Pminus * (eye(3) - K * H)' + K * R * K';
    % Save data for plotting
    posArray = [posArray x(1)];
    xhatArray = [xhatArray xhat];
    yArray = [yArray y];
    Varminus = [Varminus Pminus(1,1)];
    Varplus = [Varplus Pplus(1,1)];
    KArray = [KArray K];
end

% Plot the results
close all;
k = 1 : N;
plot(k, yArray-posArray, 'r:');
hold;
plot(k, xhatArray(1,:)-posArray, 'b-');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('position');
legend('measurement error', 'estimation error');

figure; hold;
for k = 1 : N-1
    plot([k-1 k], [Varplus(k) Varminus(k+1)]);
    plot([k k], [Varminus(k+1) Varplus(k+1)]);
end
set(gca,'FontSize',12); set(gcf,'Color','White'); set(gca,'Box','on');
xlabel('time step');
ylabel('position estimation error variance');

           

2. MATLAB仿真：示例5.2

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

function DiscreteKFAlt

% Simulate a discrete-time scalar Kalman filter.

tf = 10; % final time

F = 1; % state transition matrix
H = 1; % measurement matrix
Q = 1; % process noise covariance
R = 1; % measurement noise covariance

x = 0; % initial true state
xhat = 0; % initial estimate of x
Pplus = 1; % initial estimation error covariance

xArray = [];
xhatArray = [];
KArray = [];
PArray = [];

randn('state', 2);

for t = 0 : tf
   % Simulate the system
   x = F * x + sqrt(Q) * randn;
   y = H * x + sqrt(R) * randn;
   % Kalman filter
   Pminus = F * Pplus * F' + Q;
   K = Pminus * H' * inv(H * Pminus * H' + R);
   K = (1 + sqrt(5)) / (3 + sqrt(5)); % steady state Kalman gain
   xhat = F * xhat;
   xhat = xhat + K * (y - H * xhat);
   Pplus = (1 - K * H) * Pminus * (1 - K * H)' + K * R * K';
   % Save data for later
   xArray = [xArray; x];
   xhatArray = [xhatArray; xhat];
   KArray = [KArray; K];
   PArray = [PArray; Pminus];
end

% Plot results
close all;
t = 0 : tf;

figure;
plot(t, xArray, 'r-', t, xhatArray, 'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time');
legend('true state', 'estimated state');

figure;
plot(t, KArray, 'r-', t, PArray, 'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time');
legend('Kalman Gain', 'Estimation Covariance');

disp(['RMS estimation error = ', num2str(std(xArray - xhatArray))]);
           

>> DiscreteKFAlt
RMS estimation error = 0.56826
           

3. MATLAB仿真：示例5.3（1）

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

function [x1Array, xhatArray, KArray] = DiscreteKFEx2(Q)

% Simulate a discrete-time Kalman filter with a mismodel condition.
% We design the filter under the assumption that we are trying to estimate
% a constant bias state. In reality the state is a ramp.
% Performance can be improved by progressively increasing Q from 0 to 10.
% This illustrates the effectiveness of adding fictitious process noise 
% to the assumed model. Fictitious process noise can compensate for modeling errors.
% INPUTS:
%   Q = fictitious process noise

if ~exist('Q', 'var')
    Q = 0;
end

tf = 50; % final time

F = 1; % Assumed state matrix
H = 1; % Assumed measurement matrix
L = 1; % Assumed process noise matrix

x = [0; 10]; % initial true state. x(2) is a constant.
xhat = 0; % initial estimate of x(1)
Pplus = 1; % initial estimation error covariance
R = 1; % covariance of measurement noise

x1Array = [];
xhatArray = [];
KArray = [];

for t = 0 : tf
   % Simulate the system
   x(1) = x(1) + x(2);
   y = x(1) + sqrt(R) * randn;
   % Kalman filter
   Pminus = F * Pplus * F' + Q;
   K = Pminus * H' * inv(H * Pminus * H' + R);
   xhat = F * xhat;
   xhat = xhat + K * (y - H * xhat);
   Pplus = (eye(1) - K * H) * Pminus * (eye(1) - K * H)' + K * R * K';
   % Save data for later
   x1Array = [x1Array; x(1)];
   xhatArray = [xhatArray; xhat];
   KArray = [KArray; K];
end

% Plot results
close all;
t = 0 : tf;

figure;
plot(t, x1Array, 'r-', t, xhatArray, 'b--');
title(['Q = ',num2str(Q)], 'FontSize', 14);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time');
legend('true state', 'estimated state');

figure;
plot(t, KArray);
title(['Kalman Gain; Q = ', num2str(Q)], 'FontSize', 14);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time');
axis([0 tf 0 1]);

           

4. MATLAB仿真：示例5.3（2）

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

function DiscreteKFEx2Plot

% Create plots for example in Discrete Kalman Filter chapter.

[x1Array, xhat100, K100] = DiscreteKFEx2(1.00);
[x1Array, xhat010, K010] = DiscreteKFEx2(0.10);
[x1Array, xhat001, K001] = DiscreteKFEx2(0.01);
[x1Array, xhat000, K000] = DiscreteKFEx2(0.00);

t = 0 : 50;
close all;
figure; hold on;
plot(t, x1Array, 'k');
plot(t, xhat100, 'b*--');
plot(t, xhat010, 'rd-');
plot(t, xhat001, 'mo:');
plot(t, xhat000, 'k+');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time');
legend('true state', 'xhat (Q = 1)', 'xhat (Q = 0.1)', 'xhat (Q = 0.01)', 'xhat (Q = 0)');
set(gca,'box','on');

figure; hold on;
plot(t, K100, 'b*--');
plot(t, K010, 'rd-');
plot(t, K001, 'mo:');
plot(t, K000, 'k+');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time'); 
legend('Q = 1', 'Q = 0.1', 'Q = 0.01', 'Q = 0');
set(gca,'box','on');

           

5. 小结

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

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