---

前言

  介绍一些常用的状态估计算法，重点介绍卡尔曼滤波的原理及实现，本文需要一定高等数学、线性代数与概率论的基础

---

一、贝叶斯滤波

  贝叶斯滤波（Bayesian filtering） 是一种概率推断的方法，用于估计随时间推移的系统状态。它基于贝叶斯定理和递归贝叶斯估计的概念，通过使用先验概率和观测数据，更新状态的后验概率。
  贝叶斯滤波通常用于在有噪声或不完全观测的情况下，进行对系统状态估计。在如目标跟踪、机器人定位和导航、语音识别等领域有着广泛应用。贝叶斯滤波比较适用于有限状态空间的问题

1. 对于有限状态空间的问题，贝叶斯滤波是一种非常有效的方法，并且相对容易转换为代码实现。由于状态空间是有限的，我们可以使用离散的概率分布或概率矩阵来表示状态和观测之间的转移概率，其应用方式有离散贝叶斯滤波、静态二值贝叶斯滤波和粒子滤波器。例：判断门是开的还是关的，其状态空间只有两种情况，开和关。
2. 对于无限状态空间问题，则不适合用贝叶斯来估计，在无限状态空间中，由于状态的数量是无穷的。对对上一时刻的所有状态进行积分是办不到的事情，通常会考虑使用其他类型的滤波器或估计方法，如KF、EKF、UKF、IF、EIF这类高斯滤波器。例：判断某一个变量的值，这个值可以是任意的数，其状态空间就是无限的。

  常见的贝叶斯滤波算法包括卡尔曼滤波（Kalman Filter）和粒子滤波（Particle Filter）。卡尔曼滤波适用于线性系统和高斯噪声，而粒子滤波则适用于非线性系统和非高斯噪声。

二、卡尔曼滤波

2.1 KF简介

  卡尔曼滤波（Kalman Filter） 是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法，你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。

  卡尔曼滤波属于是基于贝叶斯最大后验估计的推论，它假设系统的状态与观测误差都是高斯分布，并且系统的动态模型和测量模型均为线性的。在卡尔曼滤波中预测和更新步骤的计算，可以通过线性方程和协方差矩阵的更新来实现，从而大大简化了计算过程，并且内存占用量低，推理速度快，比较适合资源受限制的场景。卡尔曼滤波算法分为两步:

1. 预测：根据上一时刻(k-1时刻) 的后验估计值来估计当前时刻(k时刻) 的状态，得到k时刻的先验估计值;

2. 更新：使用当前时刻的测量值来更正预测阶段估计值，得到当前时刻的后验估计值。

2.2 基本线性模型

  卡尔曼滤波是一种高效率的递归滤波器，其应用基于以下三个假设前提：

• 当前时刻状态只和上一时刻状态有关
• 模型和系统均满足线性关系
• 引入的噪声符合高斯分布

  基于上述假设，我们可以得到一离散线性动态系统的模型如下所示：
$$x_k=F{x}_{k-1}+B{u}_{k-1}+w_k\text{\hspace{1em}(1-1)}$$
$$z_k=H{x}_k+v_k\text{\hspace{1em}(1-2)}$$

  式1-1为系统状态方程，式1-2为系统观测方程，其参数如下：

• $x_k$：表示k时刻的真实值，是待估计的值，例如位置、速度
• $x_{k-1}$：表示k-1时刻的真实值，即上一时刻的真实值
• $u_{k-1}$：表示k-1时刻的控制输入量
• $w_k$：表示过程噪声
• $z_k$：表示k时刻的观测值，即测量值
• $v_k$：表示测量噪声
• $F$：表示状态转移矩阵
• $B$：表示控制矩阵
• $H$：表示观测转移矩阵

  由于噪声符合高斯分布，设均值为0，协方差矩阵分别为Q和R，则有$p(w_k)\sim N(0,Q)$ 和 $p(v_k)\sim N(0,R)$，关于协方差矩阵下面2.3.2小节介绍

2.3 KF公式推导

2.3.1 预测值

  为了更好的理解上面模型，我们假设状态值$x$为矢量，这里以运动模型的速度$v$和位置$p$为例，则有
$$x=\left[\begin{array}{c}p\\ v\end{array}\right]$$
  当然$x$并不一定为二维数据，比如在温度系统是一维，只有温度参数，也可以在三维系统。此时再假设加速度为$a$，根据速度位置计算公式有：
$$\begin{gathered} v_t=v_{t-1}+a\Delta t \\ {p}_t=p_{t-1}+v_{t-1}\Delta t+\frac{1}{2}a(\Delta t{)}^2 \end{gathered}$$

  则上式1-1可以变为
$$\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]a_{t-1}+w_t$$
  此时有$F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$，$B=\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]$

  同理对于式1-2，由于线性关系，我们假设测量与实际值关系为1:1，由于符号冲突，这里设观测误差为$c$，可变为
$$\left[\begin{array}{c}{z}_t(p)\\ z_t(v)\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]+c_t$$
  此时有$H=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

  在实际情况中，过程噪声$w_k$与测量噪声$v_k$是未知的，所以$x_k$充满了不确定性。此时再回到卡尔曼滤波的递归特性，它可以不停的修正观测值。由于测量值包含噪声，此时我们就可以忽略噪声，而将其融入到转移矩阵中

  我们使用上一时刻的最优估计值${\tilde{x}}_{k-1}$来替代真实值，并将${\bar{x}}_k$称为当前时刻通过过程模型得到的预测值，也叫做先验估计值，此时得到我们最终想要的公式：
$${\bar{x}}_k=F_k{\tilde{x}}_{k-1}+B_k{u}_{k-1}\text{\hspace{1em}(2-1)}$$

  同理观测值的预测有
$$z_k=H_k{\tilde{x}}_k$$

2.3.2 先验误差协方差矩阵

  首先定义两个变量：我们把真实值和预测值的差叫做先验误差${\bar{e}}_k$，并且把真实值和估计值的差叫做后验误差${\tilde{e}}_k$，即
$$\begin{gathered} {\bar{e}}_k=x_k-{\bar{x}}_k \\ {\tilde{e}}_k=x_k-{\tilde{x}}_k \end{gathered}$$
  除此之外，我们继续使用上一小节的速度位置模型来帮助理解。卡尔曼滤波假设两个变量（位置和速度在这个例子中）都是随机的，并且服从高斯分布。那么我们可以引入协方差$\sigma$，来表示速度$v$与位置$p$之间相关程度，记为
$$\sigma =Cov(p,v)$$
  再将协方差变成协方差矩阵$\Sigma$有
$$Cov(x)=\Sigma =\left[\begin{array}{cc}{\sigma }_{pp}& {\sigma }_{pv}\\ {\sigma }_{vp}& {\sigma }_{vv}\end{array}\right]$$
  由于${\sigma }_{pv}={\sigma }_{vp}$，很明显有$\Sigma ={\Sigma }^T$。将式1-1与式2-1代入到先验误差${\bar{e}}_k$里有
$$\begin{array}{rl}{\bar{e}}_k& =x_k-{\bar{x}}_t\\ & =F{x}_{k-1}+B{u}_{k-1}+w_k-F{\tilde{x}}_{k-1}-B{u}_{k-1}\\ & =F(x_{k-1}-{\tilde{x}}_{k-1})+w_k\\ & =F{\tilde{e}}_{k-1}+w_k\end{array}$$
  恰好得到先验误差${\bar{e}}_k$与后验误差${\tilde{e}}_k$的关系，再将上式代入到公式$Cov(Ax)=A\Sigma A^T$，由于$w_k$的协方差矩阵为Q（2.2小节有设），此时我们假设$e_k$的协方差矩阵为P，那么有
$$\begin{array}{rl}{P}_k& =Cov({\bar{e}}_k)\\ & =Cov(F{\tilde{e}}_{k-1}+w_k)\\ & =Cov(F{\tilde{e}}_{k-1})+Cov(w_k)\\ & =F{P}_{k-1}{F}^T+Q\end{array}$$
  此时得到先验误差协方差矩阵${\bar{P}}_k$公式
$${\bar{P}}_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(2-2)}$$

2.3.3 卡尔曼增益

  先介绍两种误差：估计误差$e_{est}$和测量误差$e_{mea}$，假设卡尔曼增益用$K$来标示，则有
$$K_k=\frac{e_{es{t}_{k-1}}}{e_{es{t}_{k-1}}+e_{me{a}_{k-1}}}$$
  这里我们用一维高斯分布来帮助理解，每个变量都有一个均值$\mu$，表示随机分布的中心（最可能的状态），以及方差${\sigma }^2$，表示不确定性，有
$$N(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
  假设预测值符合分布$N(x,{\mu }_0,{\sigma }_0)$，测量值符合分布$N(x,{\mu }_1,{\sigma }_1)$，那么最优估计值就是两个分布相融合的地方

  这里我们直接将两个分布函数相乘，即$N(x,{\mu }_0,{\sigma }_0)\cdot N(x,{\mu }_1,{\sigma }_1)$，可以得到分布$N(x,{\mu }^{\prime },{\sigma }^{\prime })$，有
$$\begin{gathered} {\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2} \\ {\sigma }^{\prime 2}={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2} \end{gathered}$$
  将上面相同的因子用$k$标示：
$$k=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}$$
  很显然$k$就是上面的卡尔曼增益，从而有
$$\begin{gathered} {\mu }^{\prime }={\mu }_0+k({\mu }_1-{\mu }_0) \\ {\sigma }^{\prime 2}={\sigma }_0^2-k{\sigma }_0^2 \end{gathered}$$
  将其拓展到高维变成矩阵形式，则有预测部分的高斯分布和测量部分的高斯分布
$$\begin{gathered} ({\mu }_0,{\sigma }_0)\to (\overrightarrow{{\mu }_0},{\Sigma }_0)=(H_k{\bar{x}}_k,H_k{\bar{P}}_k{H}_k^T) \\ ({\mu }_1,{\sigma }_1)\to (\overrightarrow{{\mu }_1},{\Sigma }_1)=(z_k,R_k) \end{gathered}$$
  从而可以将卡尔曼增益变成矩阵形式
$$K^{\prime }=\frac{H_k{\bar{P}}_k{H}_k^T}{H_k{\bar{P}}_k{H}_k^T+R_k}$$
  其他同样可以变为
$$\begin{gathered} H_k{\tilde{x}}_k=H_k{\bar{x}}_k+K^{\prime }(z_k-H{\bar{x}}_k) \\ {H}_k{P}_k{H}_k^T=H_k{\bar{P}}_k{H}_k^T-K^{\prime }{H}_k{\bar{P}}_k{H}_k^T \end{gathered}$$
  最后等式左乘$H_k^{-1}$，得到最终的公式
$$K={\bar{P}}_k{H}_k^T(H_k{\bar{P}}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(2-3)}$$

2.3.4 最优估计值

  实际上将$H_k{\tilde{x}}_k=H_k{\bar{x}}_k+K^{\prime }(z_k-H{\bar{x}}_k)$左乘$H_k^{-1}$就可以得到最优估计值：
$${\tilde{x}}_k={\bar{x}}_k+K_k(z_k-H{\bar{x}}_k)$$
  不过上面是从观测角度出发，如果我们从预估值角度出发，实际上生活中很多算法的基础都是取平均。举个例子，有k个时刻数据，分别为$a_1,a_2\dots a_k$，则对k+1时刻的估计值为
$$\begin{array}{rl}y& =\frac{1}{k}(a_1+a_2+\dots +a_k)\\ & =\frac{k-1}{k}\cdot \frac{1}{k-1}(a_1+a_2+\dots +a_{k-1})+\frac{1}{k}{z}_k\\ & =\frac{k-1}{k}{y}_{k-1}+\frac{1}{k}{a}_k\\ & =y_{k-1}+\frac{1}{k}(a_k-y_{k-1})\end{array}$$
  此时把最优估计值${\tilde{x}}_k$和先验估计值${\bar{x}}_k$代入到$y$和$y_{k-1}$，再用G把$\frac{1}{k}$取代，G为系数矩阵，有
$${\tilde{x}}_k={\bar{x}}_k+G(a_k-{\bar{x}}_k)$$
  很明显有估计值 = 预测值 + 权重 x 观测误差，类似加权平均算法。此时对G探讨一下发现
$$\begin{gathered} 若G\to 1，则{\tilde{x}}_k=a_k \\ 若G\to 0，则{\tilde{x}}_k={\bar{x}}_k \end{gathered}$$

  如果我们忽略噪声$v_k$有
$$z_k=H{x}_k\to x_k=H^{-1}{z}_k$$
  将$x_k$代入$a_k$，并将G用KH替代有
$${\tilde{x}}_k={\bar{x}}_k+KH(H^{-1}{z}_k-{\bar{x}}_k)$$
  此时得到最终的最优估计值计算公式：
$${\tilde{x}}_k={\bar{x}}_k+K_k(z_k-H_k{\bar{x}}_k)\text{\hspace{1em}(2-4)}$$

2.3.5 后验误差协方差矩阵

  同最优估计值一样，将上面$H_k{P}_k{H}_k^T=H_k{\bar{P}}_k{H}_k^T-K^{\prime }{H}_k{\bar{P}}_k{H}_k^T$左右都乘$H_k^{-1}$，就可以得到后验误差协方差矩阵$P_k$
$$P_k={\bar{P}}_k-K_k{H}_k{\bar{P}}_k$$
  实际上有了最优估计值，我们也可以反推卡尔曼增益和后验误差协方差矩阵。与2.3.2小节的先验误差协方差矩阵类似，后验误差协方差矩阵P有
$$P=E[{\tilde{e}}_k\cdot {\tilde{e}}_k^T]$$
  因为${\tilde{e}}_k=x_k-{\tilde{x}}_k$，将${\tilde{x}}_k={\bar{x}}_k+K(z_k-H{\bar{x}}_k)$代入有
$$\begin{array}{rl}{\tilde{e}}_k& =x_k-{\tilde{x}}_k\\ & =x_k-{\bar{x}}_k-K(z_k-H{\bar{x}}_k)\\ & =x_k-{\bar{x}}_k-K(H{x}_k+v_k-H{\bar{x}}_k)\\ & =(I-KH)(x_k-{\bar{x}}_k)-K{v}_k\\ & =(I-KH){\bar{e}}_k-K{v}_k\end{array}$$
  代入上式有
$$\begin{array}{rl}P& =E[((I-KH){\bar{e}}_k-K{v}_k)((I-KH){\bar{e}}_k-K{v}_k{)}^T]\\ & =E[(I-KH){\bar{e}}_k{\bar{e}}_k^T(I-KH{)}^T-(I-KH){\bar{e}}_k{v}_k^T-K{v}_K{\bar{e}}_k^T(I-KH{)}^T+K{v}_k{v}_k^T{K}^T]\end{array}$$
  由于${\bar{e}}_k$和$v_k$不相关，相互独立，所以协方差为0。又因为$p(v_k)\sim N(0,R)$，则有
$$\begin{array}{rl}P& =(I-KH)E[{\bar{e}}_k{\bar{e}}_k^T](I-KH{)}^T-KE[v_k{v}_k^T]K^T\\ & =(I-KH){\bar{P}}_k(I-H^T{K}^T)-KR{K}^T\\ & ={\bar{P}}_k-KH{\bar{P}}_k-{\bar{P}}_k{H}^T{K}^T+K(H{\bar{P}}_k{H}^T+R)K^T\end{array}$$
  我们目标是最小化后验误差，根据MSE准则，可以选择$P$的迹作为损失函数，记作$tr(P)$，然后对K求偏导数，这里先引用下面几个公式
$$\begin{gathered} \frac{\partial tr(A\cdot B)}{\partial A}=B^T和\frac{\partial tr(B\cdot A)}{\partial A}=B^T \\ \to \frac{\partial tr(AB{A}^T)}{\partial A}=(B{A}^T+B^T{A}^T{)}^T=A(B+B^T) \end{gathered}$$
  由于$P$是自相关矩阵，所以$P^T=P$，同理${\bar{P}}^T=\bar{P}$，于是有
$$\begin{array}{rl}\frac{\partial tr(P)}{\partial K}& =\frac{\partial tr({\bar{P}}_k)}{\partial K}-\frac{\partial tr(KH{\bar{P}}_k)}{\partial K}-\frac{\partial tr({\bar{P}}_k{H}^T{K}^T)}{\partial K}+\frac{\partial tr(K(H{\bar{P}}_k{H}^T+R)K^T)}{\partial K}\\ & =0-(H{\bar{P}}_k{)}^T-({\bar{P}}_k{H}^T{)}^T+K[(H{\bar{P}}_k{H}^T+R)+(H{\bar{P}}_k{H}^T+R{)}^T]\\ & =-2{\bar{P}}_k{H}^T+2K(H{\bar{P}}_k{H}^T+R)\end{array}$$
  令$\frac{\partial tr(P)}{\partial K}=0$，则可以得到卡尔曼增益
$$K={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+R{)}^{-1}$$
  再将K带入到P进行化简有
$$P={\bar{P}}_k-KH{\bar{P}}_k-{\bar{P}}_k{H}^T{K}^T+{\bar{P}}_k{H}^T{K}^T$$
  于是得到我们最终的公式
$$P_k=(I-K_k{H}_k){\bar{P}}_k\text{\hspace{1em}(2-5)}$$

2.4 KF算法使用

  整个卡尔曼算法就是上面5个公式，但是光有理论没用呀，我们怎么把它应用到工程里呢？此时我们可以将整个卡尔曼滤波算法变成一个函数形式如下：
$$\begin{array}{rl}\text{Algorithm}& \text{Kalman\_Filter}({\tilde{x}}_{k-1},P_{k-1},u_k,z_k):\\ & {\bar{x}}_k=F_k{\tilde{x}}_{k-1}+B_k{u}_k\\ & {\bar{P}}_k=F_k{P}_{k-1}{F}_k^T+Q_k\\ & K={\bar{P}}_k{H}_k^T(H_k{\bar{P}}_k{H}_k^T+R_k{)}^{-1}\\ & {\tilde{x}}_k={\bar{x}}_k+K_k(z_k-H_k{\bar{x}}_k)\\ & P_k=(I-K_k{H}_k){\bar{P}}_k\\ \text{return}& {\tilde{x}}_k,P_k\end{array}$$
  可以看见输入有4个变量，输出有2个值，可能有人一看有6个值瞬间就蒙了。但是我们细心观察一下，发现实际上只需要关注3个值就行

• 输入：控制向量$u_k$（如开关控制前进后退）和测量向量$z_k$（如传感器反馈值）
• 输出：时刻k的置信度，均值为${\tilde{x}}_k$

  那有人好奇其他3个值干嘛呢？实际上又回到卡尔曼滤波的递归调用特性，每次进行计算都是调用上次的输入，先用前两个公式进行预测，然后再用后面三个公式进行状态更新，最后把更新的值作为下一次调用的输入即可

2.5 MATLAB验证

  假设我们要研究的对象是一个房间的温度，其温度大概在25℃左右，会小幅度波动，每隔一分钟采样一次，以 t 表达时序。问题是要通过卡尔曼滤波估算房间真实温度。这个实例的代表意义是，他是一个一维的线性高斯系统问题。

  受光照、空气流动影响，真实温度会随环境变化，存在过程噪声。其状态转移方程：x(t) = A(t)⋅x(t-1) + B(t)⋅u(t) + ε(t)，其中A=1，B=0，过程噪声ε(t)是均值为0、方差Q=0.01的高斯分布

  用温度计测量，误差为±0.5℃，并从产品说明上得知其方差为0.25，也就是说有测量噪声。其观测方程：z(t) = C(t).x(t) + δ(t)，其中C=1，测量噪声δ均值为0、方差R=0.25

  由于卡尔曼滤波还需要上一时刻的一些输入，此时我们可以假设第 t-1 时刻温度估计值x(t-1)为25.1℃，方差 Σ(t-1)=0.1^2=0.01，假定第 t 时刻温度测量值为24.9℃

%% 根据系统描述，初始化一些值
clc;clear;
N = 300;                    % 采样次数

X = zeros(1,N);             % 状态值 真值
Z = zeros(1,N);             % 观测值 z(t)

X_kf = zeros(1,N);          % kf算法里的状态均值 μ(t)
P_kf = zeros(1,N);          % kf算法里的状态方差 Σ(t)

A = 1;                      % 状态转移方程相关
C = 1;                      % 测量方程相关
I = eye(1);                 % 单位矩阵

R = 0.01;                   % 过程噪声方差
Q = 0.25;                   % 测量噪声方差

% 初值给定
X(1) = 25.1;
Z(1) = 24.9;
X_kf(1) = Z(1);             % 通过第一个观测值来更新初始状态均值 μ(1)
P_kf(1) = 0.01;             % 这里直接将过程噪声方差作为初始方差（估摸着）


%% 根据噪声，模拟实际数据 和 测量数据
W = sqrt(R)*randn(1,N);     % 过程噪声 ε(t)
V = sqrt(Q)*randn(1,N);     % 测量噪声 δ(t)
for t = 2:N
    X(t) = A*X(t-1) + W(t);
    Z(t) = C*X(t) + V(t);
end


%% 卡尔曼滤波
for t = 2:N
    X_pre = A*X_kf(t-1);                    % 预测状态均值 _μ(t)          
    P_pre = A*P_kf(t-1)*A' + R;                % 预测状态方差 _Σ(t)
    K = P_pre*C'/(C*P_pre*C' + Q);                  % 更新卡尔曼增益 K
    X_kf(t) = X_pre + K*(Z(t) - C*X_pre);   % 更新状态均值 μ(t)
    P_kf(t) = (I - K*C)*P_pre;              % 更新状态方差 Σ(t)
end


%% 分析误差项
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for t=1:N
    Err_Messure(t)=abs(Z(t)-X(t));
    Err_Kalman(t)=abs(X_kf(t)-X(t));
end


%% 画图
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,X,'-r',t,Z,'-k',t,X_kf,'-g');
legend('real','measure','kalman extimate');         
xlabel('sample time (min)');
ylabel('temperature (℃)');
title('Kalman Filter Simulation');

figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');         
xlabel('sample time (min)');
ylabel('error (℃)');
title('Error Analysis');

figure('Name','Kalman Filter Variance','NumberTitle','off');
plot(t,P_kf,'-b');       
xlabel('sample time (min)');
ylabel('Variance');
title('Kalman Filter Variance');

2.5 Python验证

  假设对理想的一维匀加速直线运动模型，配有不精确的imu和不精确的gps，进行位置观测，这就是应用的上面速度位置模型，是一个二维的线性高斯系统，所以程序以矩阵形式体现，具体看下面代码

# -*- coding: utf-8 -*-

import numpy as np
import matplotlib.pyplot as plt

t = np.linspace(1,100,100) # 在1~100s内采样100次
u = 0.6 # 加速度值，匀加速直线运动模型
v0 = 5 # 初始速度
s0 = 0 # 初始位置
X_true = np.array([[s0], [v0]])
size = t.shape[0] + 1
dims = 2 # x, v, [位置, 速度]

Q = np.array([[1e1,0], [0,1e1]]) # 过程噪声的协方差矩阵，这是一个超参数
R = np.array([[1e4,0], [0,1e4]]) # 观测噪声的协方差矩阵，也是一个超参数。
# R_var = R.trace()
# 初始化
X = np.array([[0], [0]]) # 估计的初始状态，[位置, 速度]，就是我们要估计的内容，可以用v0，s0填入，也可以默认为0，相差越大，收敛时间越长
P = np.array([[0.1, 0], [0, 0.1]]) # 先验误差协方差矩阵的初始值，根据经验给出
# 已知的线性变换矩阵
F = np.array([[1, 1], [0, 1]]) # 状态转移矩阵
B = np.array([[1/2], [1]]) # 控制矩阵
H = np.array([[1,0],[0,1]]) # 观测矩阵

# 根据理想模型推导出来的真实位置值，实际生活中不会存在如此简单的运动模型，真实位置也不可知，本程序中使用真值的目的是模拟观测噪声数据和测量噪声数据
# 对于实际应用的卡尔曼滤波而言，并不需要知道真实值，而是通过预测值和观测值，来求解最优估计值，从而不断逼近该真值
real_positions = np.array([0] * size)
real_speeds = np.array([0] * size)
real_positions[0] = s0
# 实际观测值，通过理论值加上观测噪声模拟获得，初值即理论初始点加上观测噪声
measure_positions = np.array([0] * size)
measure_speeds = np.array([0] * size)
measure_positions[0] = real_positions[0] + np.random.normal(0, R[0][0]**0.5)
# 最优估计值，也就是卡尔曼滤波输出的真实值的近似逼近，同样地，初始值由观测值决定
optim_positions = np.array([0] * size)
optim_positions[0] = measure_positions[0]
optim_speeds = np.array([0] * size)

for i in range(1,t.shape[0]+1):
    # 根据理想模型获得当前的速度、位置真实值（实际应用中不需要），程序中只是为了模拟测试值和比较
    w = np.array([[np.random.normal(0, Q[0][0]**0.5)], [np.random.normal(0, Q[1][1]**0.5)]])
    X_true = F @ X_true + B * u + w
    real_positions[i] = X_true[0]
    real_speeds[i] = X_true[1]
    v = np.array([[np.random.normal(0, R[0][0]**0.5)], [np.random.normal(0, R[1][1]**0.5)]])
    # 观测矩阵用于产生真实的观测数据，注意各量之间的关联
    Z = H @ X_true + v
    # 以下是卡尔曼滤波的整个过程
    X_ = F @ X + B * u
    P_ = F @ P @ F.T + Q
    # 注意矩阵运算的顺序
    K = P_@ H.T @ np.linalg.inv(H @ P_@ H.T + R)
    X = X_ + K @ (Z - H @ X_)
    P = (np.eye(2) - K @ H ) @ P_
    # 记录结果
    optim_positions[i] = X[0][0]
    optim_speeds[i] = X[1][0]
    measure_positions[i] = Z[0]
    measure_speeds[i] = Z[1]
    
t = np.concatenate((np.array([0]), t))
plt.plot(t,real_positions,label='real positions')
plt.plot(t,measure_positions,label='measured positions')    
plt.plot(t,optim_positions,label='kalman filtered positions')

plt.legend()
plt.show()

plt.plot(t,real_speeds,label='real speeds')
plt.plot(t,measure_speeds,label='measured speeds')    
plt.plot(t,optim_speeds,label='kalman filtered speeds')

plt.legend()
plt.show()

三、扩展卡尔曼滤波

3.1 EKF原理

  扩展卡尔曼滤波(Extended Kalman Filter, EKF)，是KF滤波对非线性的一种拓展。其对非线性系统进行线性化，使用泰勒展开式，略去二阶及以上项(数据较小)，得到一个近似的线性化模型，然后应用卡尔曼滤波完成估计。

  首先了解一下泰勒展开式
$$f(x)=f(x_0)+\frac{f^{\prime }(x_0)}{1!}(x-x_0)+\frac{f^{\prime \prime }(x_0)}{2!}(x-x_0{)}^2+\cdots +\frac{f^{(n)}(x_0)}{n!}(x-x_0{)}^n+R_n$$
  其中$R_n$为佩亚诺(Peano)余项
$$R_n(x)=o[(x-x_0{)}^n]$$
  然后我们再看一下非线性系统模型有
$$\begin{gathered} x_k=f(x_{k-1},u_k)+w_k \\ {z}_k=h(x_k)+v_k \end{gathered}$$

• $f$：非线性状态转移函数
• $h$：非线性测量函数

  其他参数与KF一致，其中$f(x)$和$h(x_k)$泰勒展开有
$$\begin{gathered} f(x)=f(\tilde{x})+\frac{\partial f}{\partial x}(x-\tilde{x}) \\ h(x)=h(\bar{x})+\frac{\partial h}{\partial x}(x-\bar{x}) \end{gathered}$$
  然后就可以得到EKF的算法
$$\begin{array}{rl}\text{Extended}\_& \text{Kalman\_Filter}({\tilde{x}}_{k-1},P_{k-1},u_k,z_k):\\ & {\bar{x}}_k=f({\tilde{x}}_{k-1},u_k)\\ & {\bar{P}}_k=F_k{P}_{k-1}{F}_k^T+Q_k\\ & K={\bar{P}}_k{H}_k^T(H_k{\bar{P}}_k{H}_k^T+R_k{)}^{-1}\\ & {\tilde{x}}_k={\bar{x}}_k+K_k(z_k-h({\bar{x}}_k))\\ & P_k=(I-K_k{H}_k){\bar{P}}_k\\ \text{return}& {\tilde{x}}_k,P_k\end{array}$$
  其中$F$和$H$为函数$f$和$h$的雅克比矩阵。雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此，雅可比矩阵类似于多元函数的导数。所以有
$$F_k={\frac{\partial f}{\partial x}|}_{{\tilde{x}}_{k-1},u_k}=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \cdots & \frac{\partial f_n}{\partial x_n}\end{array}\right]$$
$$H_k={\frac{\partial h}{\partial x}|}_{{\bar{x}}_k}=\left[\begin{array}{ccc}\frac{\partial h_1}{\partial x_1}& \cdots & \frac{\partial h_1}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial h_n}{\partial x_1}& \cdots & \frac{\partial h_n}{\partial x_n}\end{array}\right]$$

3.2 MATLAB实现

1. 一维温度系统

%% 根据系统描述，初始化一些值
clc;clear;
N=50;                       % 采样次数

X = zeros(1,N);             % 状态值 真值
Z = zeros(1,N);             % 观测值 z(t)
Xekf = zeros(1,N);          % kf算法里的状态均值 μ(t)
Pekf = zeros(1,N);          % kf算法里的状态方差 Σ(t)

R = 1;                      % 过程噪声方差
Q = 10;                     % 测量噪声方差

% 初值给定
X(1) = 0.1;
Z(1) = X(1)^2/20;
Xekf(1) = Z(1);             % 通过第一个观测值来更新初始状态均值 μ(1)
Pekf(1) = eye(1);           % 初始方差（估摸着）


%% 根据噪声，模拟实际数据 和 测量数据
W = sqrt(R)*randn(1,N);
V = sqrt(Q)*randn(1,N);
for t = 2:N
    X(t) = 0.5*X(t-1) + 2.5*X(t-1)/(1 + X(t-1)^2) + 8*cos(1.2*t) + W(t);
    Z(t) = X(t)^2/20 + V(t);
end


% 函数 f(X) = 0.5*X + 2.5*X/(1 + X^2) 对 X 的偏导数为：
% ∂f/∂X = f'(X) = 0.5 + (2.5 - 2.5*X^2)/ (1 + X^2)^2

% 函数 f(X) = X^2/20 对 X 的偏导数为：
% ∂f/∂X = f'(X) = x / 10


%% 卡尔曼滤波
for t=2:N
    X_pre = 0.5*Xekf(t-1) + 2.5*Xekf(t-1)/(1 + Xekf(t-1)^2) + 8*cos(1.2*t);   % 预测状态均值 _μ(t)，对应g(u(t), _μ(t-1))

    G = 0.5 + (2.5 - 2.5*X_pre^2)/(1 + X_pre^2)^2;        % 一阶线性化状态方程，得到状态矩阵 A 的雅克比矩阵 G
    P_pre = G*Pekf(t-1)*G' + R;           % 预测状态方差 _Σ(t)

    H = X_pre/10;                         % 一阶线性化观测方程，得到状态矩阵 C 的雅克比矩阵 H。
    K = P_pre*H'/(H*P_pre*H' + Q);        % 更新卡尔曼增益 K

    Z_pre = X_pre^2/20;                   % 预测观测值，用于计算真实测量向量z(t)的偏差，对应 h(_μ(t))
    Xekf(t) = X_pre + K*(Z(t) - Z_pre);   % 更新状态均值 μ(t)
    Pekf(t) = (eye(1) - K*H)*P_pre;       % 更新状态方差 Σ(t)
end


%% 分析误差项
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for t=1:N
    Err_Messure(t)=abs(Z(t)-X(t));
    Err_Kalman(t)=abs(Xekf(t)-X(t));
end


%% 画图
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,X,'-r.',t,Z,'-k.',t,Xekf,'-g.');
legend('real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');

figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k.');
legend('messure error','kalman error');         
xlabel('sample time');
ylabel('error');
title('Error Analysis');

figure('Name','Kalman Filter Variance','NumberTitle','off');
plot(t,Pekf,'-b');       
xlabel('sample time');
ylabel('Variance');
title('Kalman Filter Variance');

2. 位置矩阵

%% 根据系统描述，初始化一些值
clc;clear;
T = 1;                        % ∆t
N = 60;                       % 采样次数

X = zeros(4,N);                 % 状态值 真值
Z = zeros(1,N);                 % 观测值 z(t)
Xstation = [200,0,300,0];           % 观测基站的位置
Xekf = zeros(4,N);              % kf算法里的状态均值 μ(t)
Pekf = zeros(4,4,N);            % kf算法里的状态方差 Σ(t)


A = [1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];  % 状态转移方程相关


R = 1e-3*diag([0.5,1,0.5,1]) ;  % 过程噪声的方差   
Q = 5;                          % 测量噪声方差

% 初值给定
X(:,1) = [-100,2,200,20];
Z(1) = Dist(X(:,1),Xstation);
Xekf(:,1) = X(:,1);             % 更新初始状态均值 μ(1)，这里假设我们知道初始的实际位置
Pekf(:,:,1) = eye(4);           % 初始方差（估摸着）


%% 根据噪声，模拟实际数据 和 测量数据
for t = 2:N
    X(:,t) = A*X(:,t-1) + sqrtm(R)*randn(4,1);  % 注意这里噪声的协方差与B矩阵相关
    Z(t) = Dist(X(:,t),Xstation) + sqrtm(Q)*randn(1);
end


% 函数 f(X, Y) = sqrt( (X - a)^2 + (Y - b)^2 ) 对 X 和 Y 的偏导数分别为：
% ∂f/∂X = (X - a) / ( sqrt( (X - a)^2 + (Y - b)^2 ) )
% ∂f/∂Y = (Y - b) / ( sqrt( (X - a)^2 + (Y - b)^2 ) )



%% 卡尔曼滤波
for t = 2:N
    X_pre = A*Xekf(:,t-1);                  % 预测状态均值 _μ(t)，注意这里状态转移方程是线性的
    P_pre = A*Pekf(:,:,t-1)*A' + R;    % 预测状态方差 _Σ(t)

    H = [(X_pre(1,1)-Xstation(1))/Dist(X_pre, Xstation), 0, ...
        (X_pre(3,1)-Xstation(3))/Dist(X_pre, Xstation), 0]; % 一阶线性化观测方程，得到状态矩阵 C 的雅克比矩阵 H。

    K = P_pre*H'/(H*P_pre*H' + Q);       % 更新卡尔曼增益 K

    Xekf(:,t) = X_pre + K*(Z(:,t) - Dist(X_pre, Xstation));    % 更新状态均值 μ(t)
    Pekf(:,:,t) = (eye(4) - K*H)*P_pre;     % 更新状态方差 Σ(t)
end


%% 分析误差项
Err_Messure = zeros(1,N);
Err_Kalman = zeros(1,N);
for t=1:N
    Err_Messure(t) = abs(Z(t)-Dist(X(:,t),Xstation));
    Err_Kalman(t) = Dist(X(:,t),Xekf(:,t));
end


%% 画图
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(X(1,:),X(3,:),'-k.',Xekf(1,:),Xekf(3,:),'-r.');
legend('real','kalman extimate');         
xlabel('x');
ylabel('y');
title('Kalman Filter Simulation');

% figure('Name','Kalman Filter Simulation Vx','NumberTitle','off');
% plot(t,X(2,:),'-k.',t,Xekf(2,:),'-r.');
% legend('real','kalman extimate');         
% xlabel('sample time');
% ylabel('Vx');
% title('Kalman Filter Simulation Vx');
% 
% figure('Name','Kalman Filter Simulation Vy','NumberTitle','off');
% plot(t,X(4,:),'-k.',t,Xekf(4,:),'-r.');
% legend('real','kalman extimate');         
% xlabel('sample time');
% ylabel('Vx');
% title('Kalman Filter Simulation Vy');

figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k.');
legend('messure error','kalman error');         
xlabel('sample time');
ylabel('error');
title('Error Analysis');


function d=Dist(X1,X2)
    d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end

3.3 C语言实现

#define TEST_MODE

typedef struct{
   double Xacc;
   double Yacc;
   double Zacc;
   double RollRate;
   double PitchRate;
   double YawRate;
   double magX;
   double magY;
   double magZ;
   double Roll;
   double Pitch;
   double Yaw;
   double Latitude;
   double Longitude;
   double Altitude;
   double Velocity_X;
   double Velocity_Y;
   double Velocity_Z;
}IMU_Data;

IMU_Data  AHRS_Data_filtered;
IMU_Data  *pAHRS_Data_filtered=&AHRS_Data_filtered;

#define g     9.7949
#define g2    19.5898  
#define r2d   57.2958
#define d2r   0.01745 
#define pi    3.141592
#define pi2   6.283184

#define ANGLE_Update   25
#define MAG_Update     10 
#define AHRS_Run       50
 
#define var_ax	0.962361*4
#define var_ay  0.962361*4
#define var_az  0.962361*4
//#define var_ax	0.962361
//#define var_ay  0.962361
//#define var_az  0.962361


#ifdef TEST_MODE
#define var_psi 0.504924*0.05
#else
#define var_psi 0.504924 
#endif


/*
input.p_p = &(pAHRS_Data->RollRate);
input.p_q = &(pAHRS_Data->PitchRate));
input.p_r = &(pAHRS_Data->YawRate));
	
input.p_ax = &(pAHRS_Data->Xacc);
input.p_ay = &(pAHRS_Data->Yacc);
input.p_az = &(pAHRS_Data->Zacc);
	
input.p_hx = &(pAHRS_Data->magX);
input.p_hy = &(pAHRS_Data->magY);
input.p_hz = &(pAHRS_Data->magZ);
	
input.p_phi = &(pAHRS_Data->Roll);
input.p_the = &(pAHRS_Data->Pitch);
input.p_psi = &(pAHRS_Data->Yaw);	
*/

float x_e[7][1]={0.0};
float P[7][7]={0.0};
float R[3][3]={0.0};
float F[7][7]={0.0};
float Q1 = 0.0000001;
float Q2 = 0.0000000;
float test3;
float hx=0.0,hy=0.0;
float psi=0.0;
double Yaw=0.0;


void AHRS_EKF_Init()
{
    hx=pAHRS_Data_filtered->magX;
    hy=pAHRS_Data_filtered->magY;
  int i,j;
  
  psi = -atan2(-hy, -hx);
  for (i=0; i<7; i++)
    x_e[i][0] = 0.0;
  x_e[0][0]= cos(psi*0.5);
  x_e[3][0] = sin(psi*0.5);
  
  for (i=0; i<7; i++)
    for (j=0; j<7; j++)
      P[i][j]=0.0;
  P[0][0]=0.000001;
  P[1][1]=0.000001;
  P[2][2]=0.000001;
  P[3][3]=0.000001;
  P[4][4]=0.0001;
  P[5][5]=0.0001;
  P[6][6]=0.0001;

  for (i=0; i<3; i++)
    for (j=0; j<3; j++)
  R[i][j]=0.0;
  R[0][0]=var_ax;
  R[1][1]=var_ay;
  R[2][2]=var_az;
  
  for( i = 0; i < 7; i++ )
    F[i][i]=1.0;
  
  Q1 = 0.0000001;
  Q2 = 0.000000;
} 

void  MatrixOpp(float A[][3], float B[][3]) /*矩阵求逆*/ 
    { 
     float A1,A11,A12,A13,A21,A22,A23,A31,A32,A33;
     
     A1=A[0][0]*A[1][1]*A[2][2]+A[0][1]*A[1][2]*A[2][0]+A[0][2]*A[1][0]*A[2][1]-A[0][0]*A[1][2]*A[2][1]-A[0][1]*A[1][0]*A[2][2]-A[0][2]*A[1][1]*A[2][0];     
     A11=A[1][1]*A[2][2]-A[1][2]*A[2][1];
     A12=-(A[1][0]*A[2][2]-A[1][2]*A[2][0]);
     A13=A[1][0]*A[2][1]-A[1][1]*A[2][0];
     A21=-(A[0][1]*A[2][2]-A[0][2]*A[2][1]);
     A22=A[0][0]*A[2][2]-A[0][2]*A[2][0];
     A23=-(A[0][0]*A[2][1]-A[0][1]*A[2][0]);
     A31=A[0][1]*A[1][2]-A[1][1]*A[0][2];
     A32=-(A[0][0]*A[1][2]-A[0][2]*A[1][0]);
     A33=A[0][0]*A[1][1]-A[1][0]*A[0][1];
     
      B[0][0]=A11/A1;
      B[1][0]=A12/A1;
      B[2][0]=A13/A1;
      B[0][1]=A21/A1;
      B[1][1]=A22/A1;
      B[2][1]=A23/A1;
      B[0][2]=A31/A1;
      B[1][2]=A32/A1;
      B[2][2]=A33/A1;
  } 

float wraparound(float dta)
{
  //bound heading angle between -180 and 180
  if(dta >  pi) dta -= pi2;
  if(dta < -pi) dta += pi2;
  return dta;
}	 

void AHRS_EKF()
{    //GPIO_SetBits(GPIOC, GPIO_Pin_6); 
     /p1-1
      short  i=0,j=0;
      float K[7][3]={0.0};
      float H[3][7]={0.0};
      float Hpsi[1][7]={0.0};
      float Kpsi[7][1]={0.0};
      float pc, qc, rc;
      float cPHI, sPHI; 
      float norm, Bxc, Byc, invR;
      float dt, Hdt;
      float coeff1[3] = {0.0}, temp[2] = {0.0};
      static unsigned short counter = 0;
      float dTemp[4]={0};      
      dt = 0.02; 
      Hdt = 0.5*dt;      
      pc = ((pAHRS_Data_filtered->RollRate)  - x_e[4][0]) * Hdt;  
      qc = ((pAHRS_Data_filtered->PitchRate) - x_e[5][0]) * Hdt;
      rc = ((pAHRS_Data_filtered->YawRate)   - x_e[6][0]) * Hdt;  
      
      F[0][1]=(-pc); F[0][2]=(-qc); F[0][3]=(-rc);  
      F[1][0]=pc;    F[1][2]=rc;    F[1][3]=(-qc);  
      F[2][0]=qc;    F[2][1]=(-rc); F[2][3]=pc;  
      F[3][0]=rc;    F[3][1]=qc;    F[3][2]=(-pc);  
        
      dTemp[0] =  x_e[1][0]* Hdt;  F[0][4]=dTemp[0];
      dTemp[0] =  x_e[2][0]* Hdt;  F[0][5]=dTemp[0];
      dTemp[0] =  x_e[3][0]* Hdt;  F[0][6]=dTemp[0];
      dTemp[0] = -x_e[0][0]* Hdt;  F[1][4]=dTemp[0];
      dTemp[0] =  x_e[3][0]* Hdt;  F[1][5]=dTemp[0];
      dTemp[0] = -F[0][5];         F[1][6]=dTemp[0];
      dTemp[0] = -F[1][5];         F[2][4]=dTemp[0];
      dTemp[0] =  F[1][4];         F[2][5]=dTemp[0];
      dTemp[0] =  F[0][4];         F[2][6]=dTemp[0];
      dTemp[0] =  F[0][5];         F[3][4]=dTemp[0];
      dTemp[0] = -F[0][4];         F[3][5]=dTemp[0];
      dTemp[0] =  F[1][4];         F[3][6]=dTemp[0];
      
      dTemp[0] = x_e[0][0] - pc*x_e[1][0] - qc*x_e[2][0] - rc*x_e[3][0];
      dTemp[1] = x_e[1][0] + pc*x_e[0][0] - qc*x_e[3][0] + rc*x_e[2][0];
      dTemp[2] = x_e[2][0] + pc*x_e[3][0] + qc*x_e[0][0] - rc*x_e[1][0];
      dTemp[3] = x_e[3][0] - pc*x_e[2][0] + qc*x_e[1][0] + rc*x_e[0][0];       
      for(i=0;i<4;i++) 
        x_e[i][0] = dTemp[i];
        
      float F1[7][7];
      for(i=0;i<7;i++)
      {
        for(j=0;j<7;j++)
        { 
          F1[i][j]=F[j][i];//F1为F的转秩
        }
      }
      
        float mid[7][7]={0};
        for (i=0;i<7;i++)
        {  
          for(j=0;j<7;j++)
          {
            mid[i][j]=P[i][0]*F1[0][j]+P[i][1]*F1[1][j]+P[i][2]*F1[2][j]+P[i][3]*F1[3][j]+P[i][4]*F1[4][j]+P[i][5]*F1[5][j]+P[i][6]*F1[6][j];
          }
        }
        for(i=0;i<7;i++)
        {
          for(j=0;j<7;j++)
          {
            P[i][j]=F[i][0]*mid[0][j]+F[i][1]*mid[1][j]+F[i][2]*mid[2][j]+F[i][3]*mid[3][j]+F[i][4]*mid[4][j]+F[i][5]*mid[5][j]+F[i][6]*mid[6][j];
          }
        }//算出P
        
        for(i=0;i<4;i++)
	      {
          dTemp[0] = P[i][i] + Q1;
          P[i][i]=dTemp[0];
	      }
        for(i=4;i<7;i++)
	      {
          dTemp[0] = P[i][i];
          P[i][i]=dTemp[0]+Q2;
	      }
        
        counter++;
        
        float ax = pAHRS_Data_filtered->Xacc;
      	float ay = pAHRS_Data_filtered->Yacc;
      	float az = pAHRS_Data_filtered->Zacc;
        ///p1-2
        if( counter % (AHRS_Run/ANGLE_Update) == 0) 
        {//p2-1
          float y[3][1];
          float y_s[3][1];
          
          dTemp[0] = -g2*(x_e[1][0]*x_e[3][0]-x_e[0][0]*x_e[2][0]);
          y[0][0]  = dTemp[0];
          dTemp[0] = -g2*(x_e[0][0]*x_e[1][0]+x_e[2][0]*x_e[3][0]);
          y[1][0]  = dTemp[0] ;
          dTemp[0] = -g*(x_e[0][0]*x_e[0][0]-x_e[1][0]*x_e[1][0]-x_e[2][0]*x_e[2][0]+x_e[3][0]*x_e[3][0]);
          y[2][0]  = dTemp[0];
          
          y_s[0][0]=ax;
          y_s[1][0]=ay;
          y_s[2][0]=az;
          
          dTemp[0] =  g2*x_e[2][0];   H[0][0]=dTemp[0];
          dTemp[0] = -g2*x_e[3][0];   H[0][1]=dTemp[0];
          dTemp[0] =  g2*x_e[0][0];   H[0][2]=dTemp[0];
          dTemp[0] = -g2*x_e[1][0];   H[0][3]=dTemp[0];
          dTemp[0] =  H[0][3];        H[1][0]=dTemp[0];
          dTemp[0] = -H[0][2];        H[1][1]=dTemp[0];
          dTemp[0] =  H[0][1];        H[1][2]=dTemp[0];
          dTemp[0] = -H[0][0];        H[1][3]=dTemp[0];
          dTemp[0] = -H[0][2];        H[2][0]=dTemp[0];
          dTemp[0] = -H[0][3];        H[2][1]=dTemp[0];
          dTemp[0] =  H[0][0];        H[2][2]=dTemp[0];
          dTemp[0] =  H[0][1];        H[2][3]=dTemp[0];
          
          float H1[7][3]={0.0};
          for(i=0;i<7;i++)
            for(j=0;j<3;j++)
              H1[i][j]=H[j][i];//H1为H的转秩
          
          float mid2[7][3]={0.0};
          for (i=0;i<7;i++)
          {
            for(j=0;j<3;j++)
            {
              mid2[i][j]=P[i][0]*H1[0][j]+P[i][1]*H1[1][j]+P[i][2]*H1[2][j]+P[i][3]*H1[3][j]+P[i][4]*H1[4][j]+P[i][5]*H1[5][j]+P[i][6]*H1[6][j];
            }
          }
          
          float mid3[3][3]={0.0};          
          for(i=0;i<3;i++)
          {
            for(j=0;j<3;j++)
            {
              mid3[i][j]=H[i][0]*mid2[0][j]+H[i][1]*mid2[1][j]+H[i][2]*mid2[2][j]+H[i][3]*mid2[3][j]+H[i][4]*mid2[4][j]+H[i][5]*mid2[5][j]+H[i][6]*mid2[6][j];
            }        
          }
          
          for(i=0;i<3;i++)
          {
            for(j=0;j<3;j++)
            {
              mid3[i][j]+=R[i][j];
            }
          }
          
          static float invmid3[3][3]={0.0};
          float judge=0.0;       
          judge=mid3[0][0]*mid3[1][1]*mid3[2][2]+mid3[0][1]*mid3[1][2]*mid3[2][0]+mid3[0][2]*mid3[1][0]*mid3[2][1]-mid3[0][0]*mid3[1][2]*mid3[2][1]-mid3[0][1]*mid3[1][0]*mid3[2][2]-mid3[0][2]*mid3[1][1]*mid3[2][0];
          if(judge!=0)
          {
            MatrixOpp (mid3,invmid3);//求逆
          }
          
          float midd[7][3]={0.0};         
          for(i=0;i<7;i++)
          {
            for(j=0;j<3;j++)
              midd[i][j]=H1[i][0]*invmid3[0][j]+H1[i][1]*invmid3[1][j]+H1[i][2]*invmid3[2][j];
          }
          
          for(i=0;i<7;i++)
          {
            for(j=0;j<3;j++)
              K[i][j]=P[i][0]*midd[0][j]+P[i][1]*midd[1][j]+P[i][2]*midd[2][j]+P[i][3]*midd[3][j]+P[i][4]*midd[4][j]+P[i][5]*midd[5][j]+P[i][6]*midd[6][j];
          }//求出K
          
          float e[3][1]={0.0};    
          for(i=0;i<3;i++)
            e[i][0]=y_s[i][0]-y[i][0];
          
          float mid4[7][1]={0.0};
          for (i=0;i<7;i++)
            mid4[i][0]=K[i][0]*e[0][0]+K[i][1]*e[1][0]+K[i][2]*e[2][0];
          for (i=0;i<7;i++)
            x_e[i][0]=x_e[i][0]+mid4[i][0];
          //p2-2
        }                                
        
        if( counter % (AHRS_Run/MAG_Update) == 1)
        {//p3-1
          float calibrated_hx = pAHRS_Data_filtered->magX;
          float calibrated_hy = pAHRS_Data_filtered->magY;
          cPHI= cos(pAHRS_Data_filtered->Roll);
          sPHI= sin(pAHRS_Data_filtered->Roll);
          Bxc = calibrated_hx * cos(pAHRS_Data_filtered->Pitch) + (calibrated_hy * sPHI + pAHRS_Data_filtered->magZ * cPHI) * sin(pAHRS_Data_filtered->Pitch);
          Byc = calibrated_hy * cPHI - pAHRS_Data_filtered->magZ * sPHI;
          
          norm = 1.0/sqrt(x_e[0][0]*x_e[0][0]+x_e[1][0]*x_e[1][0]+x_e[2][0]*x_e[2][0]+x_e[3][0]*x_e[3][0]);
          for(i=0;i<4;i++)
          {
            dTemp[0] = x_e[i][0]*norm;
            x_e[i][0]=dTemp[0];
          }
          
          coeff1[0]= 2*(x_e[1][0]*x_e[2][0]+x_e[0][0]*x_e[3][0]);
          coeff1[1]= 1 - 2*(x_e[2][0]*x_e[2][0]+x_e[3][0]*x_e[3][0]);
          coeff1[2]= 2/(coeff1[0]*coeff1[0]+coeff1[1]*coeff1[1]);
          
          temp[0] = coeff1[1]*coeff1[2];
          temp[1] = coeff1[0]*coeff1[2];
          
          dTemp[0] = x_e[3][0]*temp[0];
          Hpsi[0][0]=dTemp[0];
          dTemp[0] = x_e[2][0]*temp[0];
          Hpsi[0][1]=dTemp[0];
          dTemp[0] = x_e[1][0]*temp[0] + 2*x_e[2][0]*temp[1];
          Hpsi[0][2]=dTemp[0];
          dTemp[0] = x_e[0][0]*temp[0] + 2*x_e[3][0]*temp[1];
          Hpsi[0][3]=dTemp[0];
          
          float Hpsi1[7][1];
          for (i=0;i<7;i++)
            Hpsi1[i][0]=Hpsi[0][i];      
          float mid4[7][1];
          for (i=0;i<7;i++)
            mid4[i][0]=P[i][0]*Hpsi1[0][0]+P[i][1]*Hpsi1[1][0]+P[i][2]*Hpsi1[2][0]+P[i][3]*Hpsi1[3][0]+P[i][4]*Hpsi1[4][0]+P[i][5]*Hpsi1[5][0]+P[i][6]*Hpsi1[6][0];
          
          invR = 1.0/(Hpsi[0][0]*mid4[0][0]+Hpsi[0][1]*mid4[1][0]+Hpsi[0][2]*mid4[2][0]+Hpsi[0][3]*mid4[3][0]+var_psi);
          dTemp[1] = atan2(coeff1[0],coeff1[1]); 
          
          for(i=0;i<7;i++) 
          {
            dTemp[0] = mid4[i][0]*invR;
            Kpsi[i][0]=dTemp[0]; 
          }
          dTemp[0] = wraparound(atan2(Byc,-Bxc) - dTemp[1]);             
          for(i=0;i<7;i++)
          {
            dTemp[2] = x_e[i][0] + Kpsi[i][0]*dTemp[0];
            x_e[i][0]=dTemp[2];
          }
          
          float mid5[3][7];
          for(i=0;i<3;i++)
            for(j=0;j<7;j++)
              mid5[i][j]=H[i][0]*P[0][j]+H[i][1]*P[1][j]+H[i][2]*P[2][j]+H[i][3]*P[3][j]+H[i][4]*P[4][j]+H[i][5]*P[5][j]+H[i][6]*P[6][j];               
          
          float mid6[7][7];
          for(i=0;i<7;i++)
            for(j=0;j<7;j++)
              mid6[i][j]=K[i][0]*mid5[0][j]+K[i][1]*mid5[1][j]+K[i][2]*mid5[2][j];
          
          for(i=0;i<7;i++)
            for(j=0;j<7;j++)
              P[i][j]=P[i][j]-mid6[i][j];
          
          float mid7[1][7];
          for(i=0;i<1;i++)
            for(j=0;j<7;j++)
              mid7[i][j]=Hpsi[i][0]*P[0][j]+Hpsi[i][1]*P[1][j]+Hpsi[i][2]*P[2][j]+Hpsi[i][3]*P[3][j]+Hpsi[i][4]*P[4][j]+Hpsi[i][5]*P[5][j]+Hpsi[i][6]*P[6][j];
          
          for (i=0;i<7;i++)
            for(j=0;j<7;j++)
              mid6[i][j]=Kpsi[i][0]*mid7[0][j];
          
          for(i=0;i<7;i++)
            for(j=0;j<7;j++)
              P[i][j]=P[i][j]-mid6[i][j];
          //p3-2
        }
        //p4-1
        norm = 1.0/sqrt(x_e[0][0]*x_e[0][0]+x_e[1][0]*x_e[1][0]+x_e[2][0]*x_e[2][0]+x_e[3][0]*x_e[3][0]);
        for(i=0;i<4;i++)
        {
          dTemp[0] = x_e[i][0]*norm;
          x_e[i][0]=dTemp[0];
        } 
        
        pAHRS_Data_filtered->Pitch = asin(-2*(x_e[1][0]*x_e[3][0]-x_e[0][0]*x_e[2][0]));
        pAHRS_Data_filtered->Roll  = atan2(2*(x_e[0][0]*x_e[1][0]+x_e[2][0]*x_e[3][0]),1-2*(x_e[1][0]*x_e[1][0]+x_e[2][0]*x_e[2][0]));
        pAHRS_Data_filtered->Yaw   = atan2(2*(x_e[1][0]*x_e[2][0]+x_e[0][0]*x_e[3][0]),1-2*(x_e[2][0]*x_e[2][0]+x_e[3][0]*x_e[3][0]));
        //Yaw_Recalculate();
        //p4-2
       //GPIO_ResetBits(GPIOC, GPIO_Pin_6);
}