【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究（Matlab代码实现）

欢迎来到本博客❤️❤️

博主优势：博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

本文目录如下：

目录

1 概述

2 运行结果

3 参考文献

4 Matlab代码实现

---

1 概述

本文包括：

1） 标准卡尔曼滤波器 2） 扩展卡尔曼滤波器 3） 双卡尔曼滤波器 4） 平方根卡尔曼滤波器
 

在所有 4 种情况下，KF 函数都接受多维系统的噪声样本作为输入，并根据噪声样本中固有的时变过程/噪声协方差生成真实系统状态的 KF 估计值。

指数加权（或未加权）移动平均线用于估计噪声测量的时变系统协方差。

标准卡尔曼滤波器是最基本的卡尔费休实现。它假设一个模型，即噪声测量包含真实的系统状态和白噪声。

扩展卡尔曼滤波器是标准卡尔曼滤波器的推广，允许用户指定非线性系统模型，然后在 EKF 执行期间迭代线性化。

双卡尔曼滤波器同时解决了两个标准卡尔曼滤波器问题：

1） 将自回归模型拟合到数据并应用卡尔曼滤波来更新 AR 模型

2） 在执行标准 KF 更新之前，在每次迭代中应用 AR 模型

平方根卡尔曼滤波是执行标准/双卡尔曼滤波的更稳健且数值稳定的方法，尤其是当感兴趣的协方差矩阵条件不佳或几乎不是正定矩阵时。平方根卡尔曼滤波思想是以平方根形式 P = U D U' 传播过程误差协方差 P，其中 U 和 D 迭代更新，P 未显式计算。这样做将保证P是正定的，从而增加KF的数值稳定性。

2 运行结果

 

 

 部分代码：

% Number of iterations  
N = 1000;

% True State
x = zeros(2,N);

% Apriori state estimates
x_apriori = zeros(2,N);

% Aposteriori state estimates
x_aposteriori = zeros(2,N);

% Apriori error covariance estimates
P_apriori = zeros(2,2,N);

% Aposteriori error covariance estimates
P_aposteriori = zeros(2,2,N);

% Measurements
z = zeros(2,N);

% Kalman Gain
K = zeros(2,2,N);
%--------------------------------------------------------------------------

%--------------------------------------------------------------------------
% Knobs to turn
%--------------------------------------------------------------------------
% True initial state
x(:,1) = [ 0         ;
           3*pi/500 ];

% Initial aposteriori state estimate
x_aposteriori(:,1) = [ 1         ;
                       1*pi/500 ];

% Process noise covariance
Q = [   0.001    0 ;
          0      0 ];

% Measurement noise covariance
R = [ 0.1    0   ;
      0    0.01 ];

end
%--------------------------------------------------------------------------

%--------------------------------------------------------------------------
% Plot Results
%--------------------------------------------------------------------------
figure

subplot(2,1,1)
% Actual state position
b = plot(2:N,x(1,2:N),'b');
hold on
% State position estimates
r = plot(2:N,x_aposteriori(1,2:N),'r');
% State measurements
g = plot(2:N,z(1,2:N),'g+');
title('Extended Kalman Filtering of a Sine Wave - Position');
legend([b r g],'True Position','Position Estimates','Position Measurements');
xlabel('Time')
ylabel('Position')
grid on

subplot(2,1,2)
% Actual state frequency
b = plot(2:N,x(2,2:N),'b');
hold on
% State frequency estimates
r = plot(2:N,x_aposteriori(2,2:N),'r');
% Frequency measurements
g = plot(2:N,z(2,2:N),'g+');
title('Extended Kalman Filtering of a Sine Wave - Frequency');
legend([b r g],'True Frequency','Frequency Estimates','Frequency Measurements');
xlabel('Time')
ylabel('Frequency')
grid on

3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]景仙林,魏永虎.自回归的卡尔曼滤波算法在UWB定位中的应用[J].地理空间信息,2023,21(05):117-119.

[2]李成城,马立森,田原,贾运红,贾曲,田伟琴,张凯.基于CLAHE与卡尔曼滤波的掘进机机载视频稳像算法[J].工矿自动化,2023,49(05):66-73.DOI:10.13272/j.issn.1671-251x.2022100002.

[3]岳兴春,彭勇,宋威,黄嘉诚,周钰琛.融合EMA和卡尔曼滤波的MEMS去噪研究与应用[J].仪表技术与传感器,2023(04):83-86+92.

4 Matlab代码实现