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