【状态估计】【卡尔曼滤波器】基本离散kalman、固定增益的kalman、平方根kalman、遗忘因子kalman、扩大P卡尔曼、自适应kalman、有限K减小kalman雷达轨迹（Matlab代码）

💥💥💞💞欢迎来到本博客❤️❤️💥💥

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

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

📋📋📋本文目录如下：🎁🎁🎁

目录

 ⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

 ⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

     或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

卡尔曼滤波器是一种用于状态估计的强大工具，特别适用于需要从不完全和不准确的数据中提取出最佳状态估计的问题。以下是您提到的几种基本类型和变种：

1. **基本离散卡尔曼滤波（Discrete Kalman Filter）**：适用于线性系统，通过对系统的状态和测量之间的动态模型进行递归更新，以估计系统的状态。

2. **固定增益的卡尔曼滤波（Fixed-Gain Kalman Filter）**：通常用于情况比较简单，系统模型不太变化的情况下。在这种情况下，滤波器的增益（即卡尔曼增益）是固定的，不会根据测量误差和系统动态进行调整。

3. **平方根卡尔曼滤波（Square Root Kalman Filter）**：与标准卡尔曼滤波相比，平方根滤波器具有更好的数值稳定性和数值精度。通过对协方差矩阵的分解，平方根滤波器可以避免舍入误差的累积，并且更适合于实时应用。

4. **遗忘因子卡尔曼滤波（Forgotten Factor Kalman Filter）**：这种滤波器引入了一个遗忘因子，用于自适应地调整历史测量的权重。较旧的测量被赋予较低的权重，以便更好地适应系统动态的变化。

5. **自适应卡尔曼滤波（Adaptive Kalman Filter）**：在某些情况下，系统的动态模型可能会随时间发生变化，这种滤波器能够自适应地调整其模型和参数，以适应这种变化。

6. **有限K减小卡尔曼滤波（Finite K Reduction Kalman Filter）**：这种滤波器通过减小卡尔曼增益矩阵中的某些项来降低计算复杂度，适用于需要节省计算资源的情况。

7.“P扩展卡尔曼滤波”（Extended Kalman Filter，EKF）是卡尔曼滤波的一种变体，用于非线性系统的状态估计。标准的卡尔曼滤波仅适用于线性系统，但许多实际系统的动态模型通常是非线性的。因此，为了处理这些非线性系统，可以使用扩展卡尔曼滤波。

扩展卡尔曼滤波通过在状态转移和观测方程中进行线性化，将非线性系统模型转化为线性系统模型的近似，然后应用标准的卡尔曼滤波方法进行状态估计。具体步骤如下：

 扩展卡尔曼滤波通过这种方式在非线性系统中进行状态估计，虽然它是一种近似方法，但通常在实际应用中能够有效地处理许多实际问题，尤其是在缺乏更复杂非线性滤波方法的情况下。

这些都是卡尔曼滤波器的变种，用于解决不同类型的问题和应用场景。选择适合特定问题的滤波器类型通常取决于系统动态的性质、测量噪声的特征以及计算资源的限制等因素。

📚2 运行结果

 

部分代码：

%% 基本离散kalman滤波
Xkf=kalman(Xkf,u_1,Z,H,P0,Q,R,phi,Tau,N);

%% 限制K减小的kalman滤波
Xkf_rk=kalman_restrain_K(Xkf_rk,u_1,Z,H,P0,Q,R,phi,Tau,N);

%% 带遗忘因子的kalman滤波
Xkf_ff=kalman_forgetting_factor(Xkf_ff,u_1,Z,H,P0,Q,R,phi,Tau,N);

%% 扩大P的kalman滤波
Xkf_Sk=kalman_Sk(Xkf_Sk,u_1,Z,H,P0,Q,R,phi,Tau,N);

%误差分析
for i=1:N

    Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差
    Err_rkKalmanFilter(i)=RMS(X(:,i),Xkf_rk(:,i));%滤波后的误差
    Err_ffKalmanFilter(i)=RMS(X(:,i),Xkf_ff(:,i));%滤波后的误差
    Err_adKalmanFilter(i)=RMS(X(:,i),Xkf_Sk(:,i));%滤波后的误差
end
mean_Observation=mean(Err_Observation);
mean_KalmanFilter=mean(Err_KalmanFilter);
mean_fixKalmanFilter=mean(Err_rkKalmanFilter);
mean_ffKalmanFilter=mean(Err_ffKalmanFilter);
mean_adKalmanFilter=mean(Err_adKalmanFilter);

%% 画图
figure
hold on;box on;
t=(0:1:N-1);
plot(t,S(1,:),'-','LineWidth',1);%理论轨迹
plot(t,X(1,:),'--','LineWidth',1);%实际轨迹
plot(t,Z(1,:),'-o','LineWidth',1);%观测轨迹
plot(t,Xkf(1,:),':','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_rk(1,:),'-.','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_ff(1,:),'-x','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_Sk(1,:),'--s','LineWidth',2);%卡尔曼滤波轨迹
% M=M';
% plot(M(1,:),'k','LineWidth',1);%一步预测轨迹
legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','限制k减小滤波后轨迹','带遗忘因子卡尔曼滤波','扩大P的卡尔曼滤波');
xlabel('横坐标 T/s');
ylabel('纵坐标 X/m');
 
figure
hold on;box on;
plot(t,Err_Observation,'-');
plot(t,Err_KalmanFilter,'--');
plot(t,Err_rkKalmanFilter,'-.');
plot(t,Err_ffKalmanFilter,'-x');
plot(t,Err_adKalmanFilter,'-s');
% legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差');
legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),...
sprintf('限制k减小增益滤波后误差%.03f',mean_fixKalmanFilter),sprintf('带遗忘因子滤波后误差%.03f',mean_ffKalmanFilter),...
sprintf('扩大P的滤波后误差%.03f',mean_adKalmanFilter));
xlabel('观测时间/s');
ylabel('误差值');

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]夏启军,孙优贤.渐消卡尔曼滤波器的最佳自适应算法及其应用[J].自动化学报, 1990, 16(3):7.DOI:CNKI:SUN:MOTO.0.1990-03-002.

[2]闫保中,郭婉露,武小平.自适应Kalman滤波器在红外夜视跟踪系统中的研究[J].应用科技, 2011, 38(5):5.DOI:10.3969/j.issn.1009-671X.2011.05.13.

🌈4 Matlab代码实现