卡尔曼滤波C语言实现（矩阵版）

卡尔曼滤波算法

这里就不详细讲解该算法，我觉得比较好的讲解，可以参考文章：
【工程师学算法】工程常用算法（二）—— 卡尔曼滤波(Kalman Filter)

随机数的产生

均匀分布随机数的产生

均匀分布的概率密度函数为：
$$f(x)=\{\begin{array}{ll}\frac{1}{b-a},& a\le x\le b\\ 0,& 其他\end{array}$$
通常用 $U(a,b)$表示。均匀分布的均值为$(a+b)/2$，方差为$(a-b{)}^2/12$。产生均匀分布随机数的方法如下：

• 由给定的初值$x_0$，用混合同余法：$$\{\begin{array}{l}{x}_i=(a{x}_{i-1}+c)modM\\ y_i=x_i/M\end{array}$$
• 通过变换$z_i=a+(b-a)y_i$产生$(a,b)$区间上的随机数$z_i$。
  C语言代码如下：

typedef float DistriType;
DistriType Uniform(DistriType a,DistriType b,long int *seed)
{
   
    DistriType t;
    *seed = 2045 * (*seed) + 1;
    *seed = *seed - (*seed / 1048576) * 1048576;
    t = (*seed) / 1048576.0;
    t = a + (b - a) * t;
    return t;
}

正态分布的随机数

正态分布的概率密度函数为
$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
通常用$N(\mu ,{\sigma }^2)$表示。式中$\mu$为均值，${\sigma }^2$为方差。正态分布也称高斯分布。
设$r$