【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(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 运行结果

【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)_第1张图片

【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)_第2张图片【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)_第3张图片 【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)_第4张图片

 【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)_第5张图片

【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)_第6张图片 部分代码:

% 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代码实现

你可能感兴趣的:(状态估计,matlab,数学建模,开发语言)