The Kalman Filter

转载：

https://blog.csdn.net/phker/article/details/48468591

https://blog.csdn.net/u012554092/article/details/78290223

How a Kalman filter works, in pictures 必看！公式全齐了

https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ 

https://www.cnblogs.com/zsychanpin/p/7136015.html

算法的核心思想是,根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量.   再 预测下一刻的量, 

里面比较突出的是观点是. 把误差纳入计算, 而且分为预测误差和测量误差两种.通称为 噪声.

 还有一个非常大的特点是,误差独立存在, 始终不受测量数据的影响.

上面的ppt有助于入门理解.

但是在编程的时候你会发现,解释里面的数值23 没有很明确的指出,是指的那个时刻的23 是预测的23 还是上一课测量的23

下面这段文字会有助于你更清晰的理解

 

卡尔曼滤波是统计学的程序表达.

要想深入理解,公式三 协方差的背后意义 需要学习统计学. 

如果仅仅是使用的话,这5个公式套进程序里面还是很容易的.

看到这里如果你明白了原理, 你再回过头看看,会发现.误差是独立存在的. 误差不受数据的影响. 误差按照统计学的协方差公式更新, 跟数据无关. 而且误差是不断变化的.  

 

Kalman的数学原理

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：

上两式子中，x(k)是k时刻的系统状态，u(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。y(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。q(k)和r(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下图给出KF算法的流程和五个核心更新方程如下：

五个更新方程为：

--------------------- 
作者：Kuekua-seu 
来源：CSDN 
原文：https://blog.csdn.net/u012554092/article/details/78290223 
版权声明：本文为博主原创文章，转载请附上博文链接！

 

原文链接 https://www.cnblogs.com/zsychanpin/p/7136015.html

这个样例模拟质点进行匀速直线运动（速度为1），然后引入一个非常大的噪声。再用卡尔曼滤波来对质点的运动状态进行轨迹。注意是匀速直线运动。所以当中不含有控制变量。

Z=(1:100); %观測值  
noise=randn(1,100); %方差为1的高斯噪声  
Z=Z+noise;  
  
X=[0; 0]; %状态  
Sigma = [1 0; 0 1]; %状态协方差矩阵  
F=[1 1; 0 1]; %状态转移矩阵  
Q=[0.0001, 0; 0 0.0001]; %状态转移协方差矩阵  
H=[1 0]; %观測矩阵  
R=1; %观測噪声方差  
  
figure;  
hold on;  
  
for i=1:100  
  
  X_ = F*X;  
  Sigma_ = F*Sigma*F'+Q;  
  K = Sigma_*H'/(H*Sigma_*H'+R);  
  X = X_+K*(Z(i)-H*X_);  
  Sigma = (eye(2)-K*H)*Sigma_;  
    
  plot(X(1), X(2), '.','MarkerSize',10); %画点，横轴表示位置。纵轴表示速度  
end

plot([0,100],[1,1],'r-'); 

 

下图给出了上述代码的执行结果。

 

可见经过最開始的几次迭代后。质点运动的状态预计就回到了正确轨迹上，并且预计的结果基本环绕在真实值附近，效果还是非常理想的。