卡尔曼滤波思路及简单示例

%X(k) = A X(k-1)+B U(k)+W(k) 线性随机微分方程
%Z(k) = H X(k)+V(k) 系统的测量值
%此二公式分别是最初的随机噪声及测量值

%X(k|k-1) = A X(k-1|k-1) + B U(k) ………(1)
%P(k|k-1) = A P(k-1|k-1) A’+ Q ……… (2)
%（1）、（2）为对系统的预测

%Kg(k) = P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)
%X(k|k) = X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1)) ……… (4)
%P(k|k) =（I - Kg(k) H）P(k|k-1) ……… (5)
%卡尔曼滤波成立条件有二：1）线性随机微分系统；2）过程和测量都是高斯白噪声

% X(k)--------k时刻系统状态，可类推X(k-1)
% U(k)--------k时刻对系统的控制量
% A、B--------系统参数
% Z(k)--------k时刻的测量值
% H-----------测量系统的参数
% W(k)、V(k)--高斯噪声，分别用Q、R表示，Q为系统过程的协方差
% X(k|k-1)----利用上一状态预测的结果
% X(k-1|k-1)--上一状态最优的结果
% P(k|k-1)----X(k|k-1)对应的协方差
% P(k-1|k-1)--X(k-1|k-1)对应的协方差
% Kg(k)-------卡尔曼增益

% 网上的例子：把房间看成一个系统，然后对这个系统建模。当然，我们建的模型不需要非
% 常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控
% 制量，所以U(k)=0。因为测量的值是温度计的，跟温度直接对应，所以H=1。

%对照上面的计算公式：首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信
%温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时
%该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是
%3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那
%里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
%（1）X(k|k-1) = X(k-1|k-1)；--------------23 = 23；
%（2）P(k|k-1) = P(k-1|k-1) + Q；----------5^2 = 3^2 + 4^2； （Q = 4）
%（3）Kg(k) = P(k|k-1)/ (P(k|k-1) + R)；---0.78 = 5^2/（5^2 + 4^2）；（R = 4）
%（4）X(k|k) = X(k|k-1) + Kg(k)(Z(k) - X(k|k-1))；
%-----------------------------------------24.56 = 23 + 0.78（25 -23）；
%（5）P(k|k) =（I - Kg(k)）P(k|k-1)--------2.35 = （1 - 0.78）* 5^2



clc;
close all;
N = 10; %模拟N个测量值
CON = 25; %假定温度是恒定的25度
X = zeros(1,N);
Z = 2^0.5 * randn(1,N) + CON; %加入均值为0，方差为2的高斯噪声后的输出
X(1) = 1; %初始K = 1时的预测值为1
P = 5; %初始K = 1时的预测值的协方差为5
Q = cov(randn(1,N)); %过程噪声协方差
R = cov(randn(1,N)); %测量噪声协方差
for K = 2:N
X(K) = X(K - 1);

%预估计K时刻状态变量的值 X(k|k-1) = X(k-1|k-1)；
P = P + Q; %对应于预估值的协方差 P(k|k-1) = P(k-1|k-1) + Q；
Kg = P/(P + R); %计算卡尔曼增益 Kg(k) = P(k|k-1)/ (P(k|k-1) + R)；
X(K) = X(K) + Kg * (Z(K) - X(K)); %更新K时刻增益 X(k|k) = X(k|k-1) + Kg(k)(Z(k) - X(k|k-1))；
P = (1 - Kg) * P; %更新K时刻的预测值协方差 P(k|k) =（I - Kg(k)）P(k|k-1)；
end
subplot(2,1,1),stem(X,'fill'),title('预测值'); %预测值图像
subplot(2,1,2),stem(Z,'fill'),title('测量值'); %测量值图像