转载：
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-');
下图给出了上述代码的执行结果。
可见经过最開始的几次迭代后。质点运动的状态预计就回到了正确轨迹上，并且预计的结果基本环绕在真实值附近，效果还是非常理想的。