扩展卡尔曼滤波的理解与对加入高斯噪声的正弦信号进行滤波实例

卡尔曼滤波

卡尔曼滤波是一个线性状态估似算法，对高斯噪声有良好的滤波效果，也可做多传感器融合算法，如九轴。具体推导网络上，网络上很多。珠玉在前我就不过多赘述了。
卡尔曼滤波包含预测、更新两大步,五大公式如下。

预测过程
$$\begin{gathered} X{}_k^{\prime }=F{\hat{X}}_{k-1}+B{u}_k \\ P{}_k^{\prime }=F{P}_{k-1}{F}^T+Q \end{gathered}$$
更新过程
$$\begin{gathered} K_k=P{H}^T(HP{}_k^{\prime }{H}^T+R{)}^{-1} \\ {\hat{X}}_k={\hat{X}}_k+K_k(z-HX{}_k^{\prime }) \\ {P}_k=(I-K_{k}H)P{}_k^{\prime } \end{gathered}$$
F–状态转移矩阵
H–状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
P–描述不确定性的协方差矩阵
u–外部输入量 ，举例对于汽车来说，速度是状态量，油门刹车则是输入量。
B是将输入转换为状态的矩阵。
Z–测量值
k-增益
R–测量噪声
Q-系统过程演化噪声
I–单位矩阵
$X{}_k^{\prime }$ ——k 时刻的模型预测值
${\hat{X}}_k$ ——K时刻的最优估似值

简单来说，最优估计其实可以理解为按照模型误差矩阵与测量噪声矩阵得到加权系数，输入量与模型预测量相加权平均得出的。

拓展卡尔曼滤波

如果观测矩阵H或和状态转移矩阵F,出现非线性项（平方、三角函数），导致变换后噪声不再符合高斯分布，卡尔曼滤波就无法适用了。

拓展卡尔曼和卡尔曼其实区别不大，当F或者H出现非线性项时，扩展卡尔曼则对非线性矩阵做一阶泰勒展开线性化（雅可比矩阵），通俗来说，就是对各个元素一阶求偏导数，使用偏导行列式作为F与H做计算协方差P与KG增益。

正弦波滤波

正弦波 Y = SIN(A)；A相位随时间递增，观测的是Y值，
状态转移矩阵 F = SIN(A)；A相位随时间递增
状态矩阵可定为 $\left(\genfrac{}{}{0ex}{}{a}{△d}\right)$
状态转移矩阵 F = $\left|\begin{array}{cc}1& 1\\ 0& 1\end{array}\right|$
观测雅可比矩阵则为
H = $\left|\begin{array}{cc}COS(A)& 0\end{array}\right|$

对于正弦波SIN（A）扩展卡尔曼滤波，五个公式则变成

预测过程
$$\begin{gathered} X{}_k^{\prime }=F{\hat{X}}_{k-1}+B{u}_k \\ P{}_k^{\prime }=F{P}_{k-1}{F}^T+Q \end{gathered}$$
更新过程
$$\begin{gathered} K_k=P{H}^T(HP{}_k^{\prime }{H}^T+R{)}^{-1} \\ {\hat{X}}_k={\hat{X}}_k+K_k(z-h（X{}_k^{\prime }）) \\ {P}_k=(I-K_{k}H)P{}_k^{\prime } \end{gathered}$$

        //状态矩阵 x 【α，△d】   转移矩阵 f 【1,1；0，1】  观测矩阵 h()【y = sinα】    Q【Q,0;0;Q】
        public float EFKUpdate(KFP kfp, float input)
        {
        	//状态转移
            kfp.X[0] += (float)kfp.X[1];
            //kfp.X[0] = (float)Math.Sin(a);
            //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
            kfp.P2[0] = kfp.P1[0] + kfp.P1[1] + kfp.P1[2] + kfp.P1[3] + kfp.Q;
            kfp.P2[1] = kfp.P1[1] + kfp.P1[3];
            kfp.P2[2] = kfp.P1[2] + kfp.P1[3];
            kfp.P2[3] = kfp.P1[3] + kfp.Q;
       
            //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
            kfp.kg2[0] = kfp.P2[0] * (float)Math.Cos(kfp.X[0]) / (kfp.P2[0]*(float)Math.Cos(kfp.X[0])* (float)Math.Cos(kfp.X[0]) + kfp.R);
            kfp.kg2[1] = kfp.P2[2]  *  (float)Math.Cos(kfp.X[0]) / ( kfp.P2[0] *  (float)Math.Cos(kfp.X[0]) * (float)Math.Cos(kfp.X[0]) + kfp.R);
            //更新协方差方程: 本次的系统协方差付给 kfp.LastP 为下一次运算准备。
            kfp.P1[0] = (1 - kfp.kg2[0] * (float)Math.Cos(kfp.X[0])) * kfp.P2[0];
            kfp.P1[1] = (1 - kfp.kg2[0] *(float)Math.Cos(kfp.X[0])) * kfp.P2[1];
            kfp.P1[2] = (kfp.P2[2] -  kfp.kg2[1] * (float)Math.Cos(kfp.X[0]) *kfp.P2[0]);
            kfp.P1[3] = kfp.P2[3] -  kfp.kg2[1] * (float)Math.Cos(kfp.X[0]) * kfp.P2[1];
            //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
            kfp.X[0] = kfp.X[0] + kfp.kg2[0] * ((input) - (float)Math.Sin(kfp.X[0]));
            kfp.X[1] = kfp.X[1] + kfp.kg2[1] * ((input) - (float)Math.Sin(kfp.X[0]));

        //    Console.WriteLine("{0:f}\t{1:f}\r\n", kfp.X[0],kfp.X[1] );
            return (float)Math.Sin(kfp.X[0]);
        }

#参考文章
卡尔曼滤波 – 从推导到应用(一)](https://blog.csdn.net/heyijia0327/article/details/17487467)
自动驾驶中扩展卡尔曼滤波器的应用（Extended Kalman Filter）(https://blog.csdn.net/weixin_42737442/article/details/105208175)
线性卡尔曼滤波算法及示例