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