《最优状态估计-卡尔曼，H∞及非线性滤波》：第8章 时间连续卡尔曼滤波

前言

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

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

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

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

1. MATLAB仿真：示例8.4

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

function ContEx(a1, a2, q11, q12, q22, r1, r2)

% Continuous Kalman filter example for a two-state problem.

% Try different initial conditions.
p11 = 2; p12 = 1; p22 = 2;
p11 = 0; p12 = 0; p22 = 0;

PArr = [];
dt = 0.01;
tf = 3;
% Plot the time varying solution.
for t = 0 : dt : tf
    p11dot = 2 * a1 * p11 - p11^2 / r1 - p12^2 / r2 + q11;
    p12dot = (a1 + a2) * p12 - p11 * p12 / r1 - p12 * p22 / r2 + q12;
    p22dot = 2 * a2 * p22 - p12^2 / r1 - p22^2 / r2 + q22;
    p11 = p11 + p11dot * dt;
    p12 = p12 + p12dot * dt;
    p22 = p22 + p22dot * dt;
    PArr = [PArr ; p11 p12 p22];
end
close all
t = 0 : dt : tf;
plot(t, PArr(:, 1), t, PArr(:, 2), t, PArr(:, 3));
grid;
legend('p11', 'p12', 'p22');
% Compute the steady state solution.
Cond1 = (a1 ~= a2) && (q12 ~= 0);
Cond2 = (a1 == a2) && (a1 < 0) && (q12 ~= 0) && (q11*q22-q12*q12 == 0);
Cond3 = (a1 == a2) && (a1 > 0) && (q12 ~= 0) && (q11*q22-q12*q12 == 0);
if Cond1 || Cond3
    gamma1 = q11 / r1 + a1^2;
    gamma2 = q22 / r2 + a2^2;
    p12 = q12 / ( gamma1 + gamma2 + 2 * ( gamma1 * gamma2 - q12^2 / r1 / r2 )^(1/2) )^(1/2);
    p11 = r1 * ( a1 + ( gamma1 - p12^2 / r1 / r2 )^(1/2) );
    p22 = r2 * ( a2 + ( gamma2 - p12^2 / r1 / r2 )^(1/2) );
    disp(['p11 = ', num2str(p11), ', p12 = ', num2str(p12), ', p22 = ', num2str(p22)]);
    lambda = eig([p11 p12; p12 p22]);
    disp(['Eigenvalues of P = ', num2str(lambda(1)), ', ', num2str(lambda(2))]);
end
if Cond2 || Cond3
    gamma3 = -a1 + ( a1^2 + q11 / r1 + q22 / r2 )^(1/2);
    p11 = q11 / gamma3;
    p22 = q22 / gamma3;
    p12 = q12 / gamma3;
    disp(['p11 = ', num2str(p11), ', p12 = ', num2str(p12), ', p22 = ', num2str(p22)]);
    lambda = eig([p11 p12; p12 p22]);
    disp(['Eigenvalues of P = ', num2str(lambda(1)), ', ', num2str(lambda(2))]);
end
if ~Cond1 && ~Cond2 && ~Cond3 
    disp(['p11 = ', num2str(p11), ', p12 = ', num2str(p12), ', p22 = ', num2str(p22)]);
    lambda = eig([p11 p12; p12 p22]);
    disp(['Eigenvalues of P = ', num2str(lambda(1)), ', ', num2str(lambda(2))]);
end

2. 小结

大部分卡尔曼滤波的应用都由数字计算机实现，所以对于时间连续的两侧值去讨论卡尔曼滤波似乎有些多余。然而，还是有应用到连续时间卡尔曼滤波的可能（例如在模拟电路中）。此外，时间连续卡尔曼滤波器的推导从教学的角度看也还是非常有意义的。

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

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