《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——卡尔曼滤波器

FuShenxiao

《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——卡尔曼滤波器 [复制链接]

本帖最后由 FuShenxiao 于 2025-3-6 14:21 编辑

卡尔曼滤波器是一种最优化的、递归的、数字处理的算法。它兼具滤波器和观测器的特性。在某些应用中给，卡尔曼滤波器主要用于对系统状态的估计和预测，因此被视为状态观测器；在其他应用中，卡尔曼滤波器主要用于对噪声信号的滤波和去噪，因此被视为信号滤波器。

卡尔曼滤波器通过将多个传感器或测量其的数据进行融合，利用概率论和线性系统理论对状态进行估计和预测，从而有效地解决了由不确定性带来的问题。它能够根据先验知识和测量信息来递归地更新状态估计，并提供对未来状态的最优预测。

卡尔曼滤波器的研究模型

考虑一个带有过程噪声的线性离散系统的状态空间方程

$x_{[k]}=Ax_{[k-1]}+Bu_{[k-1]}+w_{[k-1]}$

其中过程噪声服从于期望为0、协方差矩阵为Qc的正态分布，即

$w\sim N(0,Q_c)$

其中协方差矩阵Qc为

$Q_c=E(ww^T)=E\begin{pmatrix} \begin{bmatrix} w_1\\ \vdots\\ w_n \end{bmatrix} & \begin{bmatrix} w_1 & \cdots & w_n \end{bmatrix} \end{pmatrix}= \begin{bmatrix} \sigma _{w_1}^2 & \cdots & \sigma_{w_1 w_n}\\ \vdots & \ddots & \vdots\\ \sigma_{w_n w_1} & \cdots & \sigma_{w_n}^2 \end{bmatrix}$

如果过程噪声相互独立，则可以化简为对角矩阵，其对角元素为每一个状态变量过程噪声的方差，即

$Q_c= \begin{bmatrix} \sigma _{w_1}^2 & \cdots & 0\\ \vdots & \ddots & \vdots\\ 0 & \cdots & \sigma_{w_n}^2 \end{bmatrix}$

定义观测向量

$z_{[k]}=H_mx_{[k]}+v_{[k]}$

其中Hm为观测矩阵，这一矩阵代表了观测值与状态向量之间的线性关系。v[k]为测量噪声，它服从于期望为0、协方差矩阵为Rc的正态分布，与Qc类似，如果测量噪声项目独立，则协方差矩阵为对角矩阵

$R_c=\begin{bmatrix} \sigma_{v_1}^2 & \cdots & 0\\ \vdots & \ddots & \vdots\\ 0 & \cdots & \sigma_{v_n}^2 \end{bmatrix}$

由于存在过程噪声w和测量噪声v，无法得出精确的状态向量x[k]。通过设计卡尔曼滤波器将这两个式子中的结果进行数据融合，便可以得到一个相对准确的估计值。

首先定义先验状态估计

$\hat{x}_{[k]}^{-}=A\hat{x}_{[k-1]}+Bu_{[k-1]}$

对测量值做同样的处理，引入测量估计

$z_{[k]}=H_m\hat{x}_{mea_{[k]}}$

于是测量估计为

$\hat{x}_{mea_{[k]}}=H_m^{-1}z_{[k]}$

将以上二者结合，定义k时刻的后验状态估计

$\hat{x}_{[k]}=\hat{x}_{[k]}^{-}+G_{[k]}(\hat{x}_{mea_{[k]}}-\hat{x}_{[k]}^{-})$

将观测向量代入，得到后验状态估计

$\hat{x}_{[k]}=\hat{x}_{[k]}^{-}+G_{[k]}(H_m^{-1}-\hat{x}_{[k]}^{-})=\hat{x}_{[k]}^{-}+GH_m^{-1}(z_{[k]}-H_m\hat{x}_{[k]}^{-})$

令 $K_{[k]}=GH_m^{-1}$ ，整理得到

$\hat{x}_{[k]}=\hat{x}_{[k]}^{-}+K_{[k]}(z_{[k]}-H_m\hat{x}_{[k]}^{-})$

其中K[k]被称为卡尔曼增益，通过它将先验状态估计与测量值融合在一起得到更加准确的后验状态估计。特别的

$K_{[k]}\rightarrow 0\Rightarrow \hat{x}_{[k]}\rightarrow \hat{x}_{[k]}^{-}$

这种情况说明测量噪声远大于过程噪声，因此系统更加相信先验状态估计。得到的后验状态估计将很靠近先验状态估计。相反，如果测量误差很小，则

$K_{[k]}\rightarrow H_m^{-1}\Rightarrow \hat{x}_{[k]}\rightarrow H_m^{-1}z_{[k]}$

系统将更加线性测量值。

卡尔曼增益求解

引入先验状态估计误差和后验状态估计误差

$e_{[k]}^{-}=x_{[k]}-\hat{x}_{[k]}^{-}$

$e_{[k]}=x_{[k]}-\hat{x}_{[k]}$

代入得到先验状态估计误差

$e_{[k]}^{-}=Ae_{[k-1]}+w_{[k-1]}$

先验状态估计误差的协方差矩阵为

$P_{[k]}^{-}=E(e_{[k]}^{-}(e_{[k]}^{-})^T)$

代入得到后验状态估计误差

$e_{[k]}=(I-K_{[k]}H_m)e_{[k]}^{-}-K_{[k]}v_{[k]}$

后验状态估计误差的协方差矩阵为

$P_{[k]}=E(e_{[k]}e_{[k]}^T)=\begin{bmatrix} \sigma_{e_{1_{[k]}}}^2 & \cdots & \sigma_{e_{1_{[k]}}e_{n_{[k]}}}\\ \vdots & \ddots & \vdots\\ \sigma_{e_{n_{[k]}}e_{1_{[k]}}} & \cdots & \sigma_{e_{n_{[k]}}}^2 \end{bmatrix}$

卡尔曼滤波器的设计目标为找到迹最小的后验状态估计误差协方差矩阵的卡尔曼增益，即

$K_{[k]}^{\ast}=\mathop{\arg\min}\text{Tr} (P_{[k]})$

经过推导得到最优估计的卡尔曼增益

$K_{[k]}=\frac{P_{[k]}^{-}H_m^T}{H_mP_{[k]}^{-}H_m^T+R_c}$

其中Hm和Rc都可以当作已知量，因此只需要得到先验状态估计的协方差矩阵即可求得卡尔曼增益，于是得到

$P_{[k]}^{-}=AP_{[k-1]}A^T+Q_c$

$P_{[k]}=(I-K_{[k]}H_m)P_{[k]}^{-}$

根据以上的推导，得到卡尔曼滤波器的运算过程如下

同时得到以下几点讨论

观察卡尔曼滤波器的公式，可以发现k时刻的预测和校正都只与k-1时刻的预测和k时刻的测量值相关，这是递归算法的特性，每次计算只需要保存前一时刻的状态量和协方差矩阵，而不需要保存所有历史数据。这种递归方式可以有效地节省存储空间，提高运算效率，特别是在处理大量数据时尤为明显。
若过程噪声的协方差矩阵Qc和测量噪声的协方差矩阵Rc是常数矩阵，随着时间的推移，系统收集到的测量数据增多，会使得状态估计更加准确，卡尔曼增益K[k]和估计误差协方差矩阵P[k]将趋于稳定（常数矩阵）。这一结论可以通过数值方法得到验证。因此在实际应用中，可以通过离线测试的方法预先得到稳定地卡尔曼增益和协方差矩阵。需要注意的是，现实中Qc和Rc一般很难保持常数，通常会随着环境发生改变，可以使用自适应卡尔曼滤波器来提高性能，或者采用其他的方法使用时变的噪声矩阵。
选择Qc和Rc是卡尔曼滤波器设计中非常重要的一个环节，它会直接影响滤波器的性能和稳定性。在实际应用中，可以采用以下两个步骤确定这两个重要的矩阵。

初始估计：根据系统的物理模型和传感器特性，对Qc和Rc进行初始估计。其中Rc通常可以通过观测或查询传感器的参数得到。
实验测试：使用实验数据对Qc和Rc进行调整。在实验中，需要记录系统的状态和测量值，并根据测量数据计算出卡尔曼增益和误差协方差矩阵。然后，根据卡尔曼滤波器的状态估计结果对Qc和Rc进行调整，使得滤波器的性能能够达到预期的要求。

编写线性卡尔曼滤波器程序模块框图如下

MATLAB代码实现如下

%% 输入：系统矩阵 A，B； 协方差矩阵 Q_c，R_c； 测量矩阵 H；
%%     第k-1次的： 测量值 z； 后验估计误差协方差矩阵 P； 后验估计值 x_hat； 输入 u；
%% 输出：第k次的： 后验估计值: x_hat； 后验估计误差协方差矩阵 P;
function [x_hat, x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H,z,x_hat,P,u)
%% 计算先验状态估计
    x_hat_minus = A*x_hat + B*u;
%% 计算先验估计误差协方差矩阵
    P_minus = A*P*transpose(A) + Q_c;
%% 计算卡尔曼增益
    K =  P_minus*transpose(H)/(H*P_minus*transpose(H)+R_c);
%% 更新后验估计
    x_hat = x_hat_minus + K*(z-H*x_hat_minus);
%% 后验估计误差协方差矩阵
    P = (eye(size(A,1)) - K*H)*P_minus;
end

案例分析

以无人机高度观测为例

过程噪声的协方差矩阵为

$Q_{c_a}=\begin{bmatrix} 0.05 & 0 & 0\\ 0 & 0.05 & 0\\ 0 & 0 & 0 \end{bmatrix}$

测量矩阵

$H_{m}=\begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}$

测量噪声的协方差矩阵为

$R_{c_a}=\begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 0 \end{bmatrix}$

注意，在实际编程中测量噪声的协方差矩阵Rca的右下角的0应该取一个不为0的值（不影响最终结果），因为取0的话由于矩阵不满秩，会导致计算过程中出现奇异值，在实际手推过程中，可以把Rca看作一个分块矩阵分析。

MATLAB代码实现如下

clear
clc
%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%%
% 构建系统矩阵A，n x n
A = [0 1 0; 0 0 1 ;0 0 0];
% 计算A矩阵维度
n = size (A,1);
% 构建输入矩阵B，n x p
B = [0; 1; 0];
% 计算输入矩阵维度
p = size(B,2);
% 定义测量矩阵H_m, n x n
H_m = [1 0 0; 0 1 0; 0 0 1];
% 重力加速度常数
g = 10;
C = [1, 0, 0];
D = 0;

%%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%%
% 离散时间步长
Ts = 0.1;
% 连续系统转离散系统
sys_d = c2d(ss(A,B,C,D),Ts);
% 提取离散系统A矩阵
A = sys_d.a;
% 提取离散系统B矩阵
B = sys_d.b;

%%%%%%%%%%%%%%%%%参数设计%%%%%%%%%%%%%%%%%%%%%
% 定义过程噪声协方差矩阵
Q_c = [0.05 0 0; 0 0.05 0; 0 0 0];
% 定义测量噪声协方差矩阵
R_c = [1 0 0; 0 1 0; 0 0 0.001];

%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%
% 初始化系统状态
x0 = [0; 1 ; -10];
% 初始化状态赋值
x = x0;
% 系统输入初始化
u0 = g;
% 初始输入赋值
u = u0;
% 初始化后验估计
x_hat0 = [0; 1; -10];
% 初始化后验估计赋值
x_hat = x_hat0;
% 初始化后验估计误差协方差矩阵
P0 = [1 0 0;0 1 0; 0 0 0];
% 初始后验估计误差协方差矩阵赋值
P = P0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps
x_history = zeros(n,k_steps);
% 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps
u_history = zeros(p,k_steps);
% 定义x_hat_history零矩阵，用于储存后验估计结果，维度n x k_steps
x_hat_history = zeros(n,k_steps);
% 定义x_hat_minus_history零矩阵，用于储存先验估计结果，维度n x k_steps
x_hat_minus_history = zeros(n,k_steps);
% 定义z_historyy零矩阵，用于测量结果，维度n x k_steps
z_history = zeros(n,k_steps);


%%%%%%%%%%%%%%%定义仿真环境%%%%%%%%%%%%%%%%%%%%
% 定义过程噪声矩阵W，维度n x k_steps
w = zeros (n,k_steps);
% 定义测量噪声矩阵V，维度n x k_steps
v = zeros (n,k_steps);
% 从文件NoiseData.csv中读取数据
% 数据来自于系统随机生成，保存为文件可以方便进行多组实验之间的对比
data = readmatrix('NoiseData.csv');
w = data(2:4,:);
v = data(6:8,:);
%%%%%%%%%%%%%生成过程与测量噪声%%%%%%%%%%%%%%%%%%
% 使用以下代码生成随机噪声，之后保存在NoiseData.csv中，方便下次读取。%%%%%%

% 定义真实的过程噪声协方差矩阵
% Q_ca = [0.05 0; 0 0.05];
% 定义真实的测量噪声协方差矩阵
% R_ca = [1 0; 0 1];
% 随机生成过程噪声
% w(1:2,:) = chol(Q_ca)* randn(2,k_steps);
% 随机生成测量噪声
% v(1:2,:) = chol(R_ca)* randn(2,k_steps);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% 仿真开始，建立for循环
for k = 1:k_steps
% 系统状态空间方程，计算实际状态变量
    x = A * x + B*u + w(:,k);
% 计算实际测量值，在实际应用中，这一项来自传感器测量
    z = H_m * x + v(:,k);
% 使用卡尔曼滤波器
    [x_hat,x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H_m,z,x_hat,P,u);
% 保存系统状态到预先定义矩阵的相应位置
    x_history (:,k+1) =  x;
% 保存测量值到预先定义矩阵的相应位置
    z_history (:,k+1) =  z;
% 保存先验估计到预先定义矩阵的相应位置
    x_hat_minus_history (:,k+1) = x_hat_minus;
% 保存后验估计到预先定义矩阵的相应位置
    x_hat_history (:,k+1) = x_hat;
end

%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%
% x1真实结果
plot (0:length(x_history)-1,x_history(1,:),'--','LineWidth',2);
hold on
% x1测量值
plot ( 0:length(z_history)-1,z_history(1,:),'*','MarkerSize',8)
ylim([-2 12]);
% x1先验估计值
plot (0:length(x_hat_minus_history)-1,x_hat_minus_history(1,:),'o','MarkerSize',8);
% x1后验估计值
plot ( 0:length(x_hat_history)-1,x_hat_history(1,:),'LineWidth',2);
legend(' 真实值 ',' 测量值 ',' 先验估计值 ',' 后验估计值 ')
set(legend, 'Location', 'southeast','FontSize', 20);
hold off;
grid on
% 计算Ems 均方误差
Ems=(x_hat_history(1,:)-x_history(1,:))*(x_hat_history(1,:)-x_history(1,:))'/k_steps;

得到仿真结果如下图所示

当过程噪声协方差矩阵不能确定时，可以首先确定一个较大的过程噪声协方差矩阵，运行并观察系统的表现，之后不断下调过程噪声并找到最合适的Qc，这就是系统识别的过程。