关于卡尔曼滤波的p阵初始值的简单分析与实验结果说明

首先，本文的大部分内容都参照或者直接照抄了这篇文章，这篇文章讲的应该是全网最细节的了，可以放心使用：
https://blog.csdn.net/ouok000/article/details/125999213#comments_25416540

由于卡尔曼滤波的p阵的协方差有效的反映了不同数据之间的联系，因此通过p阵更新将p阵进行自适应迭代，再更新了传播噪声的Q阵之后，从而可以自适应的完成协方差矩阵的自适应修正。

实际上如果上面的式子是单维度的，其实会变成我们熟悉的东西，就是数列，其实理论上来说，由于j与q都是订制，显然p阵应当会收敛到一个定值。咦，如果j=1，q=1，那p不就是发散了。哈哈哈，和大家开个玩笑而已，因为p阵的迭代会受到新的更新

由于K阵里面包含了R阵

通过而k阵里面包含了R阵，也就是将量测噪声引入了。
从而最终将量测噪声与传输噪声都加入了新的p阵的更新。

那么问题来了，P阵的更新实际上只与p的初始值有关，当量测数据一定时，理论上来说，通过该数据上，可以感性的发现量测数据之间的关系，而这个关系应当是一定的，所以协方差矩阵应当是一定的。

所以进行了以下的测试：

close all
Z=(1:1000);
noise=0.01*ones(1,1000);
Z=Z+noise;

X=[0;0];
P=[1 0;0 1];
% P=[0.1024 0.01;0.01 -0.001];
F=[1 1;0 1];
Q=[0.0001, 0.01;0.01 0.0001];
H=[1 0];
R=1;

for i=1:1000
X_=FX;
P_=FPF’+Q;
K=P_H’/(HP_H’+R);
X=X_+K(Z(i)-HX_);
P=(eye(2)-K*H)*P_;

zhuangtai(:,i) = X_;
pzhen(:,:,i) = P_;

end
figure;
hold on;
plot(1:1000, zhuangtai(1,:),‘r-’)
plot(1:1000,Z,‘g-’)

asd1 = reshape(pzhen(1,1,:),1,size(pzhen,3));
figure
plot(asd1)
asd2 = reshape(pzhen(2,2,:),1,size(pzhen,3));
figure
plot(asd2)
asd3 = reshape(pzhen(1,2,:),1,size(pzhen,3));
figure
plot(asd3)
% % figure
% % plot3(pzhen(2,2,:))
% % figure
% % plot3(pzhen(1,2,:))

代码出自B站老哥，在这里就https://www.bilibili.com/video/BV1vs411z7PX/?spm_id_from=333.337.search-card.all.click&vd_source=94998485a56909360cb6fc4617acbb98

所以可以发现
（1,1）的方差

（2,2）的地方

（1,2）的地方

这个是初始值是P=[1 0;0 1];的情况，，
看一下初始值P=[0.1024 0.01;0.01 -0.001];的情况

（1,1）的位置

（2,2）的位置

（1,2）的位置

从而可以发现，最终的协方差还是通过多次的迭代，收敛到一直，但是收敛的速度是不一样的。
这里如果有人学过三院的自动控制原理，会发现这个图形有点像反馈控制的曲线。实际上卡尔曼滤波也是一种时变反馈系统，当p阵逐渐收敛之后会变成时不变反馈系统。
PID调参的时候，比例K的系数大一点虽然超调会大一点，但是收敛速度会快很多。

所以其实有一点需要和大家说明的事情，如果我人为的将p阵直接设置为收敛的最终p阵，是不是就没有震荡，直接收敛了。
从上数的结果看，应该是没有的，大家也可以发现，最终的收敛速度反而慢了。
所以初始值的设置是有一些技巧和玄学的，大家可以自行查一下论文。我在这里就不乱发表意见了。