KF计算过程

- 已知：
 1，观测值
 2，观测值方差，可以理解为噪声，一般为高斯 
 3，初始状态
 4，状态协方差矩阵
 5，状态转移矩阵
 6，状态转移协方差矩阵
 7，观测矩阵
 8，观测噪声方差，观测噪声
 9，观测矩阵
 10，观测协方差矩阵

- 求：
 1，最新状态

- 进入迭代：
 a）：由初始状态，根据状态转移矩阵计算当前状态
 b）：由初始状态协方差计算当前状态协方差矩阵
 c）：计算kelman增益
 d）：更新状态
    I，计算偏差（观测值减去观测矩阵和当前状态的乘积）
    II，当前状态加上kelman增益乘以偏差
    III，计算值赋值作为最新状态
 e）：更新协方差矩阵
    I，kelman增益乘以观测矩阵乘以当前状态协方差矩阵
    II，当前状态协方差矩阵减去I的值
    III，计算值赋值作为最新状态协方差矩阵

demo on matlab：

clear all;
close all;
clc;

Z=(1:100); %观测值
noise=randn(1,100); %方差为1的高斯噪声
Z=Z+noise;

X=[1; 1]; %状态
P=[1 0; 0 1]; %状态 协方差矩阵
F=[1 1; 0 1]; %状态转移矩阵

Q=[0.0001, 0; 0 0.0001]; %状态转移 协方差矩阵

H=[1 0]; %观测矩阵
R = 1;
%R= [2, 0]; %观测噪声方差  观测 协方差矩阵
%R = [2,0;0,2];

figure;
hold on;

for i=1:100
  %init
  X_ = F*X;
  P_ = F*P*F'+Q;

  %kelman enhance
  K = P_*H'/(H*P_*H'+R);

  %update
  X = X_+K*(Z(i)-H*X_);

  P = (eye(2)-K*H)*P_;

  plot(X(1),X(2),'.'); %画点，横轴表示位置，纵轴表示速度
end