滤波 订阅
滤波(Wave filtering)是将信号中特定波段频率滤除的操作,是抑制和防止干扰的一项重要措施,滤波分为经典滤波和现代滤波。 展开全文
滤波(Wave filtering)是将信号中特定波段频率滤除的操作,是抑制和防止干扰的一项重要措施,滤波分为经典滤波和现代滤波。
信息
外文名
Wave filtering
用    于
抑制和防止干扰
分    类
经典滤波和现代滤波
中文名
滤波
实    质
将信号中特定波段频率滤除的操作
滤波简介
滤波是将信号中特定波段频率滤除的操作,是抑制和防止干扰的一项重要措施。是根据观察某一随机过程的结果,对另一与之有关的随机过程进行估计的概率理论与方法。滤波一词起源于通信理论,它是从含有干扰的接收信号中提取有用信号的一种技术。“接收信号”相当于被观测的随机过程,“有用信号”相当于被估计的随机过程。例如用雷达跟踪飞机,测得的飞机位置的数据中,含有测量误差及其他随机干扰,如何利用这些数据尽可能准确地估计出飞机在每一时刻的位置、速度、加速度等,并预测飞机未来的位置,就是一个滤波与预测问题。这类问题在电子技术、航天科学、控制工程及其他科学技术部门中都是大量存在的。历史上最早考虑的是维纳滤波,后来R.E.卡尔曼和R.S.布西于20世纪60年代提出了卡尔曼滤波。现对一般的非线性滤波问题的研究相当活跃。
收起全文
精华内容
参与话题
问答
  • 1、细说贝叶斯滤波-------很容易理解 2、从理论推导到代码实现 3、简单放狗比拟例子理解,卡尔曼和粒子 4、卡尔曼的推导和应用 5、从实际场景理解粒子滤波
    展开全文
  • imu数据融合 一阶滤波 卡尔曼滤波

    千次阅读 2019-02-21 10:31:20
    首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏...本帖主要介绍三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波...

    首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

    本帖主要介绍三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波算法。

    一、四元数法

    关于四元数的一些概念和计算就不写上来了,我也不懂。我能告诉你的是:通过下面的算法,可以把六个数据转化成四元数(q0、q1、q2、q3),然后四元数转化成欧拉角(P、R、Y 角)。

    虽然 MPU6050 自带的 DMP库可以直接输出四元数,减轻 STM32 的运算负担, 这里在此没有使用,因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的(http://bbs.elecfans.com/forum.ph … 4&page=1#pid3625735),DMP库需要对 I2C 函数进行修改,如 DMP 库中的 I2C 写:i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]));有4个输入变量,而 STM32 硬件 I2C 的 I2C 写为:void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr),只有 3 个输入量(这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的),所以必须进行修改,但是改固件库是一件很痛苦的事,你们应该都懂。当然,如果你用模拟 I2C 的话,是容易实现的,网上的 DMP 移植几乎都是基于模拟 I2C 的。

    复制代码

    #include<math.h>

    #include “stm32f10x.h”

    //---------------------------------------------------------------------------------------------------

    // 变量定义

    #define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计

    #define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接

    #define halfT 0.001f // 采样周期的一半

    float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素,代表估计方向

    float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差

    float Yaw,Pitch,Roll; //偏航角,俯仰角,翻滚角

    void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

    {

        float norm;
    
        float vx, vy, vz;
    
        float ex, ey, ez;  
    
        // 测量正常化
    
        norm = sqrt(ax*ax + ay*ay + az*az);      
    
        ax = ax / norm;                   //单位化
    
        ay = ay / norm;
    
        az = az / norm;      
    
        // 估计方向的重力
    
        vx = 2*(q1*q3 - q0*q2);
    
        vy = 2*(q0*q1 + q2*q3);
    
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
    
        ex = (ay*vz - az*vy);
    
        ey = (az*vx - ax*vz);
    
        ez = (ax*vy - ay*vx);
    
        // 积分误差比例积分增益
    
        exInt = exInt + ex*Ki;
    
        eyInt = eyInt + ey*Ki;
    
        ezInt = ezInt + ez*Ki;
    
        // 调整后的陀螺仪测量
    
        gx = gx + Kp*ex + exInt;
    
        gy = gy + Kp*ey + eyInt;
    
        gz = gz + Kp*ez + ezInt;
    
        // 整合四元数率和正常化
    
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
    
        // 正常化四元
    
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    
        q0 = q0 / norm;
    
        q1 = q1 / norm;
    
        q2 = q2 / norm;
    
        q3 = q3 / norm;
    
        Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
    
        Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
    
        //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值,注掉
    

    }

    要注意的的是,四元数算法输出的是三个量 Pitch、Roll 和 Yaw,运算量很大。而像平衡小车这样的例子只需要一个角(Pitch 或 Roll )就可以满足工作要求,个人觉得做平衡小车最好不用四元数法。

    二、一阶互补算法

    MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角(加速度不能得到 Yaw 角),就是说有两组 Pitch、Roll 角,到底应该选哪组呢?别急,先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,且通过算 tan() 可以得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以,不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角,需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值,把它们结合到一起,进行修正。得到 Pitch 角的程序如下:

    复制代码

    //一阶互补滤波

    float K1 =0.1; // 对加速度计取值的权重

    float dt=0.001;//注意:dt的取值为滤波器采样时间

    float angle;

    angle_ax=atan(ax/az)*57.3; //加速度得到的角度

    gy=(float)gyo[1]/7510.0; //陀螺仪得到的角速度

    Pitch = yijiehubu(angle_ax,gy);

    float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度

    {

     angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
    
     return angle;
    

    }

    互补算法只能得到一个倾角,这在平衡车项目中够用了,而在四轴飞行器设计中还需要 Roll 和 Yaw,就需要两个 互补算法,我是这样写的,注意变量不要搞混:

    复制代码

    //一阶互补滤波

    float K1 =0.1; // 对加速度计取值的权重

    float dt=0.001;//注意:dt的取值为滤波器采样时间

    float angle_P,angle_R;

    float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度

    {

     angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
    
         return angle_P;
    

    }

    float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度

    {

     angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
    
         return angle_R;
    

    }

    单靠 MPU6050 无法准确得到 Yaw 角,需要和地磁传感器结合使用。

    三、卡尔曼滤波

    其实卡尔曼滤波和一阶互补有些相似,输入也是一样的。卡尔曼原理以及什么5个公式等等的,我也不太懂,就不写了,感兴趣的话可以上网查。在此给出具体程序,和一阶互补算法一样,每次卡尔曼滤波只能得到一个方向的角度。

    复制代码

    #include<math.h>

    #include “stm32f10x.h”

    #include “Kalman_Filter.h”

    //卡尔曼滤波参数与函数

    float dt=0.001;//注意:dt的取值为kalman滤波器采样时间

    float angle, angle_dot;//角度和角速度

    float P[2][2] = {{ 1, 0 },

                 { 0, 1 }};
    

    float Pdot[4] ={ 0,0,0,0};

    float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

    float R_angle=0.5 ,C_0 = 1;

    float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

    //卡尔曼滤波

    float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

    {

        angle+=(gyro_m-q_bias) * dt;
    
        angle_err = angle_m - angle;
    
        Pdot[0]=Q_angle - P[0][1] - P[1][0];
    
        Pdot[1]=- P[1][1];
    
        Pdot[2]=- P[1][1];
    
        Pdot[3]=Q_gyro;
    
        P[0][0] += Pdot[0] * dt;
    
        P[0][1] += Pdot[1] * dt;
    
        P[1][0] += Pdot[2] * dt;
    
        P[1][1] += Pdot[3] * dt;
    
        PCt_0 = C_0 * P[0][0];
    
        PCt_1 = C_0 * P[1][0];
    
        E = R_angle + C_0 * PCt_0;
    
        K_0 = PCt_0 / E;
    
        K_1 = PCt_1 / E;
    
        t_0 = PCt_0;
    
        t_1 = C_0 * P[0][1];
    
        P[0][0] -= K_0 * t_0;
    
        P[0][1] -= K_0 * t_1;
    
        P[1][0] -= K_1 * t_0;
    
        P[1][1] -= K_1 * t_1;
    
        angle += K_0 * angle_err; //最优角度
    
        q_bias += K_1 * angle_err;
    
        angle_dot = gyro_m-q_bias;//最优角速度
    
        return angle;
    

    }

    作个总结:三种融合算法都能够输出姿态角(Pitch 和 Roll ),一次四元数法可以输出 P、R、Y 三个倾角,计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。

    转自于:https://blog.csdn.net/zsn15702422216/article/details/52223799

    展开全文
  • 谈谈MPU6050的数据融合 一阶滤波 卡尔曼滤波

    万次阅读 多人点赞 2016-08-16 18:17:13
    首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD ...

       

    首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

    本帖主要介绍三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波算法。




    一、四元数法

    关于四元数的一些概念和计算就不写上来了,我也不懂。我能告诉你的是:通过下面的算法,可以把六个数据转化成四元数(q0、q1、q2、q3),然后四元数转化成欧拉角(P、R、Y 角)。



    虽然 MPU6050 自带的 DMP库可以直接输出四元数,减轻 STM32 的运算负担, 这里在此没有使用,因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的(http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735),DMP库需要对 I2C 函数进行修改,如 DMP 库中的 I2C 写:i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]));有4个输入变量,而 STM32 硬件 I2C 的 I2C 写为:void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr),只有 3 个输入量(这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的),所以必须进行修改,但是改固件库是一件很痛苦的事,你们应该都懂。当然,如果你用模拟 I2C 的话,是容易实现的,网上的 DMP 移植几乎都是基于模拟 I2C 的。


    复制代码


    #include<math.h>

    #include "stm32f10x.h"

    //---------------------------------------------------------------------------------------------------

    // 变量定义


    #define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计

    #define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接

    #define halfT 0.001f                // 采样周期的一半


    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素,代表估计方向

    float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差


    float Yaw,Pitch,Roll;  //偏航角,俯仰角,翻滚角



    void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

    {

            float norm;

            float vx, vy, vz;

            float ex, ey, ez;  


            // 测量正常化

            norm = sqrt(ax*ax + ay*ay + az*az);      

            ax = ax / norm;                   //单位化

            ay = ay / norm;

            az = az / norm;      


            // 估计方向的重力

            vx = 2*(q1*q3 - q0*q2);

            vy = 2*(q0*q1 + q2*q3);

            vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;


            // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和

            ex = (ay*vz - az*vy);

            ey = (az*vx - ax*vz);

            ez = (ax*vy - ay*vx);


            // 积分误差比例积分增益

            exInt = exInt + ex*Ki;

            eyInt = eyInt + ey*Ki;

            ezInt = ezInt + ez*Ki;


            // 调整后的陀螺仪测量

            gx = gx + Kp*ex + exInt;

            gy = gy + Kp*ey + eyInt;

            gz = gz + Kp*ez + ezInt;


            // 整合四元数率和正常化

            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  


            // 正常化四元

            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

            q0 = q0 / norm;

            q1 = q1 / norm;

            q2 = q2 / norm;

            q3 = q3 / norm;


            Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数

            Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv

            //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值,注掉

    }




    要注意的的是,四元数算法输出的是三个量 Pitch、Roll 和 Yaw,运算量很大。而像平衡小车这样的例子只需要一个角(Pitch 或 Roll )就可以满足工作要求,个人觉得做平衡小车最好不用四元数法。




    二、一阶互补算法

    MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角(加速度不能得到 Yaw 角),就是说有两组 Pitch、Roll 角,到底应该选哪组呢?别急,先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,且通过算 tan()  可以得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以,不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角,需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值,把它们结合到一起,进行修正。得到 Pitch 角的程序如下:




    复制代码


    //一阶互补滤波

    float K1 =0.1; // 对加速度计取值的权重

    float dt=0.001;//注意:dt的取值为滤波器采样时间

    float angle;


    angle_ax=atan(ax/az)*57.3;     //加速度得到的角度

    gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度

    Pitch = yijiehubu(angle_ax,gy);


    float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度

    {

         angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);

         return angle;

    }




    互补算法只能得到一个倾角,这在平衡车项目中够用了,而在四轴飞行器设计中还需要 Roll 和 Yaw,就需要两个 互补算法,我是这样写的,注意变量不要搞混:


    复制代码


    //一阶互补滤波

    float K1 =0.1; // 对加速度计取值的权重

    float dt=0.001;//注意:dt的取值为滤波器采样时间

    float angle_P,angle_R;



    float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度

    {

         angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);

             return angle_P;

    }


    float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度

    {

         angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);

             return angle_R;

    }

    单靠 MPU6050 无法准确得到 Yaw 角,需要和地磁传感器结合使用。






    三、卡尔曼滤波

    其实卡尔曼滤波和一阶互补有些相似,输入也是一样的。卡尔曼原理以及什么5个公式等等的,我也不太懂,就不写了,感兴趣的话可以上网查。在此给出具体程序,和一阶互补算法一样,每次卡尔曼滤波只能得到一个方向的角度。




    复制代码




    #include<math.h>

    #include "stm32f10x.h"

    #include "Kalman_Filter.h"




    //卡尔曼滤波参数与函数

    float dt=0.001;//注意:dt的取值为kalman滤波器采样时间

    float angle, angle_dot;//角度和角速度

    float P[2][2] = {{ 1, 0 },

                     { 0, 1 }};

    float Pdot[4] ={ 0,0,0,0};

    float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

    float R_angle=0.5 ,C_0 = 1;

    float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;


    //卡尔曼滤波

    float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

    {

            angle+=(gyro_m-q_bias) * dt;

            angle_err = angle_m - angle;

            Pdot[0]=Q_angle - P[0][1] - P[1][0];

            Pdot[1]=- P[1][1];

            Pdot[2]=- P[1][1];

            Pdot[3]=Q_gyro;

            P[0][0] += Pdot[0] * dt;

            P[0][1] += Pdot[1] * dt;

            P[1][0] += Pdot[2] * dt;

            P[1][1] += Pdot[3] * dt;

            PCt_0 = C_0 * P[0][0];

            PCt_1 = C_0 * P[1][0];

            E = R_angle + C_0 * PCt_0;

            K_0 = PCt_0 / E;

            K_1 = PCt_1 / E;

            t_0 = PCt_0;

            t_1 = C_0 * P[0][1];

            P[0][0] -= K_0 * t_0;

            P[0][1] -= K_0 * t_1;

            P[1][0] -= K_1 * t_0;

            P[1][1] -= K_1 * t_1;

            angle += K_0 * angle_err; //最优角度

            q_bias += K_1 * angle_err;

            angle_dot = gyro_m-q_bias;//最优角速度


            return angle;

    }






    作个总结:三种融合算法都能够输出姿态角(Pitch 和 Roll ),一次四元数法可以输出 P、R、Y 三个倾角,计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。



    注:使用“匿名地面站”上位机软件,给调试过程带来了很大便捷。软件可在附件中下载。

    通过上位机可以看到,姿态融合成功!




     

    展开全文
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

    万次阅读 多人点赞 2019-04-06 16:33:48
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用...

    更新日志:

    2020.02.13:修改了第三节推导中的公式错误

    2020.03.21:修改了2.1节中的部分表述和公式加粗,补充迹的求导公式

    1 简介

    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。

    EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用卡尔曼滤波框架对信号进行滤波,因此它是一种次优滤波。

     

    2 算法介绍

    2.1 泰勒级数展开

    泰勒级数展开是将一个在x=x_{0}处具有n阶导数的函数f(x),利用关于(x-x_{0})n次多项式逼近函数值的方法。

    若函数f(x)在包含x_{0}的某个闭区间[a,b]上具有n阶导数,且在开区间(a,b)上具有(n+1)阶导数,则对闭区间[a,b]上的任意一点x,都有:

    f(x)=\frac{f({{x}_{0}})}{0!}+\frac{f'({{x}_{0}})}{1!}(x-{{x}_{0}})+...+\frac{{{f}^{(n)}}({{x}_{0}})}{n!}{{(x-{{x}_{0}})}^{n}}+{{R}_{n}}(x)

    其中{{f}^{(n)}}({{x}_{0}})表示函数f(x)x=x_{0}处的n阶导数,等式右边成为泰勒展开式,剩余的{{R}_{n}}(x)是泰勒展开式的余项,是(x-x_{0})^{n}的高阶无穷小。

    (著名的欧拉公式{{e}^{ix}}=\cos x +i\sin x就是利用{{e}^{ix}}\cos x\sin x的泰勒展开式得来的!)

    当变量是多维向量时,一维的泰勒展开就需要做拓展,具体形式如下:

    f(\mathbf{x})=f({{\mathbf{x}}_{k}})+{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}(\mathbf{x}-{{\mathbf{x}}_{k}})+\frac{1}{2!}{{(\mathbf{x}-{{\mathbf{x}}_{k}})}^{T}}H({{\mathbf{x}}_{k}})(\mathbf{x}-{{\mathbf{x}}_{k}})+{{o}^{n}}

    其中,{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}={{\mathbf{J}}_{F}}表示雅克比矩阵,\mathbf{H}({{\mathbf{x}}_{k}})表示海塞矩阵,{{\mathbf{o}}^{n}}表示高阶无穷小。

    {[\nabla f({{\bf{x}}_k})]^T} = {{\bf{J}}_F} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_n}}}}\\ {\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_n}}}} \end{array}} \right]

    {\bf{H}}({{\bf{x}}_k}) = \left[ {\begin{array}{*{20}{c}} {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_1^2}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_n}}}}\\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_2^2}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_n^2}}} \end{array}} \right]

    这里,f({{\bf{x}}_k})m维,{{\bf{x}}_k}状态向量为n维,\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_m}\partial {x_n}}} = \frac{{\partial f{{({{\bf{x}}_k})}^T}}}{{\partial {x_m}}}\frac{{\partial f({{\bf{x}}_k})}}{{\partial {x_n}}}

    一般来说,EKF在对非线性函数做泰勒展开时,只取到一阶导和二阶导,而由于二阶导的计算复杂性,更多的实际应用只取到一阶导,同样也能有较好的结果。取一阶导时,状态转移方程和观测方程就近似为线性方程,高斯分布的变量经过线性变换之后仍然是高斯分布,这样就能够延用标准卡尔曼滤波的框架。

     

    2.1 EKF

    标准卡尔曼滤波KF的状态转移方程和观测方程为

    {{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}

    {{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}

    扩展卡尔曼滤波EKF的状态转移方程和观测方程为

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}          (1)

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}             (2)

    利用泰勒展开式对(1)式在上一次的估计值\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle处展开得

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)+{{\mathbf{s}}_{k}}          (3)

    再利用泰勒展开式对(2)式在本轮的状态预测值\mathbf{\theta }_{k}^{'}处展开得

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}=h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{v}}_{k}}            (4)

    其中,{\mathbf{F}}_{k-1}{\mathbf{H}}_{k}分别表示函数f(\mathbf{\theta })h(\mathbf{\theta })\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle\mathbf{\theta }_{k}^{'}处的雅克比矩阵。

    (注:这里对泰勒展开式只保留到一阶导,二阶导数以上的都舍去,噪声假设均为加性高斯噪声)

     

    基于以上的公式,给出EKF的预测(Predict)和更新(Update)两个步骤:

    Propagation:

    \mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)

    \mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    其中的雅克比矩阵{\mathbf{F}}_{k-1}{\mathbf{H}}_{k}分别为

    {{\mathbf{F}}_{k-1}}={{\left. \frac{\partial f}{\partial \mathbf{\theta }} \right|}_{\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle }}{{\mathbf{H}}_{k}}={{\left. \frac{\partial h}{\partial \mathbf{\theta }} \right|}_{\mathbf{\theta }_{k}^{'}}}

    雅可比矩阵的计算,在MATLAB中可以利用对自变量加上一个eps(极小数),然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。

    读者可能好奇?为什么扩展卡尔曼滤波EKF的传播和更新的形式会和标准卡尔曼滤波KF的形式一致呢?以下做一个简单的推导。

     

    3 推导

    先列出几个变量的表示、状态转移方程和观测方程:

    真实值{{\mathbf{\theta }}_{k}},预测值\mathbf{\theta }_{k}^{'},估计值\left\langle {{\mathbf{\theta }}_{k}} \right\rangle,观测值{{\mathbf{z}}_{k}},观测值的预测\mathbf{\hat{z}}_{k},估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}},求期望的符号\left\langle \cdot \right\rangle

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}},     {{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}},     {{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})

    引入反馈:\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-{{{\mathbf{\hat{z}}}}_{k}} \right)=\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h(\theta _{k}^{'} )\right)      (5)

     

    OK,可以开始推导了:

    由公式(3)(4)得到以下两个等式,标为式(6)(7)

    f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )={{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)

    h({{\mathbf{\theta }}_{k}})-h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)={{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{{'}} \right)

    计算估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}},并把式子(4)(5)(7)代入,得到

    {{\mathbf{\Sigma }}_{k}}=\left\langle {{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right){{\left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right)}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{{\mathbf{K}}_{k}}{\mathbf{v}}_{k}} \right]{{\left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{{\mathbf{K}}_{k}}{\mathbf{v}}_{k}} \right]}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}}{\mathbf{R}}{\mathbf{K}}_{k}}^{T}

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\mathbf{\Sigma }_{k}^{'}{{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}}{\mathbf{R}}{\mathbf{K}}_{k}}^{T}

    其中\mathbf{\Sigma }_{k}^{'}表示真实值与与预测值之间的误差协方差矩阵。于是得到式(8)

    {{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}\mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}^{T}}}\mathbf{K}_{k}^{T}+{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}

    因为{{\mathbf{\Sigma }}_{k}}的对角元即为真实值与估计值的误差的平方,矩阵的迹(用T[]表示)即为总误差的平方和,即

    T\left[ {{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {{\mathbf{K}}_{k}}\mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'} \right]

    利用以下矩阵迹的求导公式(其中\mathbf{A}\mathbf{B}表示矩阵,\mathbf{a}表示列向量):

    Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})

    Tr(\mathbf{AB})=Tr(\mathbf{BA})

    \mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}

    要让估计值更接近于真实值,就要使上面的迹尽可能的小,因此要取得合适的卡尔曼增益{{\mathbf{K}}_{k}},使得迹得到最小,言外之意就是使得迹对{{\mathbf{K}}_{k}}的偏导为0,即

    \frac{dT\left[ {{\mathbf{\Sigma }}_{k}} \right]}{d{{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'} \right)}^{T}}=0

    这样就能算出合适的卡尔曼增益了,即

    {{\mathbf{K}}_{k}}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}{{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    代回式(8)得到

    {{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}{{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}\mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    接下来就差真实值与预测值之间的协方差矩阵\mathbf{\Sigma }_{k}^{'}的求值公式了

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle

    将以下两个等式代入到上式,

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )

    可以得到

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \left[f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]{{\left[ f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle

    {{\mathbf{\theta }}_{k}}\left\langle {{\mathbf{\theta }}_{k}} \right\rangle与观测噪声{{\mathbf{s}}_{k}}是独立的,求期望等于零;;\left\langle {{\mathbf{s}}_{k}}{{\mathbf{s}}_{k}}^{T} \right\rangle表示观测噪声的协方差矩阵,用{\mathbf{Q}}表示。于是得到

    \mathbf{\Sigma }_{_{k}}^{'}=\mathbf{F}_{k-1}\left\langle \left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right){{\left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right)}^{T}} \right\rangle {{\mathbf{F}}_{k-1}^{T}}+\left\langle {{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle \\ =\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    其中的协方差矩阵的转置矩阵就是它本身。OK,大功告成,以上就完成了全部公式的推导了。

     

    4 实际应用

    现在我们假设在海上有一艘正在行驶的船只,其相对于传感器的横纵坐标为(x;y;v_{x};v_{y})为隐藏状态,无法直接获得,而传感器可以测量得到船只相对于传感器的距离和角度(r;\theta),传感器采样的时间间隔为\Delta t,则:

    状态向量(x;y;v_{x};v_{y}),观测向量(r;\theta)

    状态转移方程和观测方程为:

    \left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{x_{k}}} \\ {{v}_{y_{k}}} \\ \end{matrix} \right]=f(\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right])+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}

    \left[ \begin{matrix} {{r}_{k}} \\ {{\theta }_{k}} \\ \end{matrix} \right]=h(\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right])+{{\mathbf{v}}_{k}}=\left[ \begin{matrix} \sqrt{x_{k}^{2}+x_{y}^{2}} \\ \arctan \frac{{{y}_{k}}}{{{x}_{k}}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}

    那么雅克比矩阵为

    {{\mathbf{F}}_{k-1}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]

    {{H}_{k}}=\left[ \begin{matrix} \frac{\partial {{r}_{k}}}{\partial {{x}_{k}}} & \frac{\partial {{r}_{k}}}{\partial {{y}_{k}}} & \frac{\partial {{r}_{k}}}{\partial {{v}_{{{x}_{k}}}}} & \frac{\partial {{r}_{k}}}{\partial {{v}_{{{y}_{k}}}}} \\ \frac{\partial {{\theta }_{k}}}{\partial {{x}_{k}}} & \frac{\partial {{\theta }_{k}}}{\partial {{y}_{k}}} & \frac{\partial {{\theta }_{k}}}{\partial {{v}_{{{x}_{k}}}}} & \frac{\partial {{\theta }_{k}}}{\partial {{v}_{{{y}_{k}}}}} \\ \end{matrix} \right]

    这里给定距离传感器的噪声均值为0,方差为10;角度传感器的噪声均值为0,方差为0.001(单位弧度);

    采样时间点为\small 100个;

    船只的初始状态为(1000;1500;5;-3),四个状态量的噪声的方差分别为(2;2;0.2;0.2)。仿真结果如下:

     

    从仿真结果可以看出,EKF在这种情形下的滤波效果还是不错的,但是在实际应用中,对于船只运动的状态转移噪声的均值\mathbf q和协方差矩阵\mathbf Q,以及传感器的观测噪声的均值\mathbf r和协方差矩阵\mathbf R,往往都是未知的,有很多情况都只有观测值而已,这样的情形下,就有必要利用观测值对噪声的统计量参数做出适当的估计(学习)。

     

    5 参数估计(参数学习)

    利用EM算法和极大后验概率估计(MAP),对未知的噪声参数做出估计,再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出,之后可能会补充,这里就先给出一种Adaptive-EKF算法的公式。

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}},     {{\mathbf{s}}_{k}}\sim \mathcal{N}(\mathbf{q},\mathbf{Q})

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}},     {{\mathbf{v}}_{k}}\sim \mathcal{N}(\mathbf{r},\mathbf{R})

    {{\mathbf{\varepsilon }}_{k}}={{\mathbf{z}}_{k}}-h(\mathbf{\theta }_{k}^{'})-{{\mathbf{r}}_{k}}

    (1)E-Step

    Propagation:

    \mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)

    \mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{G}}_{k}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    (2)M-Step

    {{\mathbf{\hat{q}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{q}}}_{k\text{-}1}}+\frac{1}{k}\left[ \left\langle {{\theta }_{k}} \right\rangle -f\left( \left\langle {{\theta }_{k-1}} \right\rangle \right) \right]

    {{\mathbf{\hat{Q}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{Q}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{K}}_{k}}{{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}\mathbf{K}_{k}^{T}+{{\mathbf{\Sigma }}_{k}}-{{\mathbf{F}}_{k-1}}{{\mathbf{\Sigma }}_{k-1}}\mathbf{F}_{k-1}^{T} \right]

    {{\mathbf{\hat{r}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{r}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{z}}_{k}}-h\left( \left\langle \theta _{k}^{'} \right\rangle \right) \right]

    {{\mathbf{\hat{R}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{R}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}-{{\mathbf{H}}_{k}}\mathbf{\Sigma }_{k}^{'}\mathbf{H}_{k}^{T} \right]

    利用以上的Adaptive-EKF算法对船只的运动做滤波跟踪,得到的效果如下图所示:

    相比于没有做参数估计的EKF滤波,可以看出,Adaptive-EKF在估计误差上要优于EKF滤波,而且,它并不需要指定状态转移噪声和观测噪声的参数,将更有利于在实际中的应用。

     

    6 总结

    EKF滤波通过泰勒展开公式,把非线性方程在局部线性化,使得高斯分布的变量在经过线性变换后仍然为高斯分布,这使得能继续把标准卡尔曼滤波KF的框架拿过来用,总体来说,EKF在函数的非线性不是很剧烈的情形下,能够具有很不错的滤波效果。但是EKF也有它的不足之处:其一,它必须求解非线性函数的Jacobi矩阵,对于模型复杂的系统,它比较复杂而且容易出错;其二,引入了线性化误差,对非线性强的系统,容易导致滤波结果下降。基于以上原因,为了提高滤波精度和效率,以满足特殊问题的需要,就必须寻找新的逼近方法,于是便有了粒子滤波PF和无迹卡尔曼滤波UKF,笔者将在接下来的博文中为读者解读。

     

    7 参考文献

    [1] 林鸿. 基于EM算法的随机动态系统建模[J]. 福建师大学报(自然科学版), 2011, 27(6):33-37. 

    [2] https://www.cnblogs.com/gaoxiang12/p/5560360.html.

    [3] https://max.book118.com/html/2017/0502/103920556.shtm.


    原创性声明:本文属于作者原创性文章,小弟码字辛苦,转载还请注明出处。谢谢~ 

    如果有哪些地方表述的不够得体和清晰,有存在的任何问题,亦或者程序存在任何考虑不周和漏洞,欢迎评论和指正,谢谢各路大佬。

    需要代码和有需要相关技术支持的可咨询QQ:297461921

    展开全文
  • mpu6050姿态解算与卡尔曼滤波(2)卡尔曼滤波

    万次阅读 多人点赞 2017-03-09 12:10:59
    卡尔曼滤波,是当前最优估计理论中十分重要的一个部分。 要全面地理解卡尔曼滤波,你需要一点统计学的知识,以及一点矩阵理论。通常最优估计理论的教材是从最小二乘估计讲起,接着讲到最小方差估计,极大似然估计...
  • 卡尔曼滤波

    千次阅读 2018-12-15 14:59:50
    文章目录卡尔曼滤波卡尔曼滤波简介Dynamic model(实时系统)目标跟踪分析卡尔曼滤波五大更新方程时间更新状态更新代码实现——小球跟踪参考资料: 卡尔曼滤波简介 卡尔曼滤波是一种高效率的递归滤波器(自回归...
  • 滤波方法及其原理, 卡尔曼滤波,互补滤波
  • 清华大学现代信号处理自己做的课程设计,绝对可以运行,大家放心,主要内容为维纳滤波以及卡尔曼滤波,维纳滤波是基于FIR进行设计的,卡尔曼滤波是基于2阶运动模型的状态方程进行设计的,直接运行Project1.m文件即可...
  • 扩展卡尔曼滤波+卡尔曼滤波

    千次阅读 2017-03-16 10:12:23
    扩展卡尔曼滤波详细推导: http://wenku.baidu.com/link?url=M4SU1ysz7q-9uZFfrdv_pkdcE1siXViqm9OCJj3PgL6PeedjUr1q9cgEKdi8iG0wDLDbRfXs-Vizmy5eGzc-FFV13ZlzQeZhyQeKhkFBkn_扩展卡尔曼滤波原理: ...
  • 所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解析解获得后验概率(KF、EKF、UKF),也可通过大数统计平均求期望的方法来获得后验概率(粒子滤波)。 KF、EKF、UKF ...
  • 一、引言 本文以rssi(接收信号强度)滤波为背景,结合卡尔曼的五个公式,设计 ...3、卡尔曼滤波过程及五个基本公式 4、公式中每个参数详细注释 5、结合rssi滤波实例设计滤波器 6、MATLAB实现滤波器 二、模型的...
  • 最近需要做一个空中飞鼠(AirMouse)项目,其中对六轴陀螺仪回传的...其中我着重学习了卡尔曼滤波和扩展卡尔曼滤波,并对扩展卡尔曼滤波进行实现。 1. 卡尔曼滤波学习 卡尔曼滤波算法是卡尔曼(Kalman)等人在20世纪
  • 卡尔曼滤波理论与实践(MATLAB版)(第四版) 莫欣德 S.格雷沃 (Mohinder S.Grewal) (作者), 安格斯 P.安德鲁斯 (Angus P.Andrews) matlab代码,卡尔曼滤波各种实例分析
  • 高通滤波和自适应LMS迭代滤波算法比较方法 卡尔曼滤波算法 FMCW探地雷达
  • 贝叶斯滤波和卡尔曼滤波的简要介绍,包括贝叶斯公式的推导,贝叶斯滤波的假设条件,卡尔曼滤波的五个方程。
  • 粒子滤波&卡尔曼滤波实例比较,可视化图像显示,部分代码注释分析
  • 高斯滤波+卡尔曼滤波+粒子滤波C++&Matlab;代码合集,详细介绍了高斯滤波、卡尔曼滤波、粒子滤波的算法原理及公式推导,并且附上了C++代码和Matlab代码两种演示验证程序。
  • 卡尔曼滤波 Kalman Filter 与维纳滤波 Wiener Filter 是什么关系?链接 数字图像去噪滤波算法性能和复杂度比较:链接 转载于:https://www.cnblogs.com/2008nmj/p/8110979.html...
  • 维纳滤波和卡尔曼滤波

    千次阅读 2017-04-24 17:54:56
    最近在学习opencv,在Videom模块中用到了卡曼滤波,看到的别人的文章比较好,仰慕,故转载: 随机信号或随机过程(random process)是普遍存在的。一方面,任何确定性信号经过测量后往往就会引入随机性误差而使该...
  • 看了一会贝叶斯滤波,记录几篇看得明白的。 https://www.cnblogs.com/FangLai-you/p/10973255.html ... 卡尔曼滤波: https://www.cnblogs.com/ycwang16/p/5999034.html 马尔科夫链: https://b...
  • 参考:细说贝叶斯滤波,很容易理解 参考一:从理论推导到代码实现, 参考二:简单放狗比拟例子理解,卡尔曼和粒子 参考三:卡尔曼的推导和应用 参考四:从实际场景理解粒子滤波 ...
  • 第三讲.贝叶斯滤波的三大概率 第四讲.连续随机变量的贝叶斯公式...卡尔曼都是期望和方差 由上一个时刻的期望和方差,推出这个时刻期望和方差的先验(预测值,两个公式) 由这个时刻期望和方差的先验再推出这个时刻期望和
  • 里面有个基于卡尔曼滤波的视频跟踪matlab程序,还有个基于粒子滤波的视频个跟踪程序,自己验证过。
  • 做语音增强了两种基本方法,kalman滤波和维纳滤波的方法。希望对学习增强的同学有帮助。
  • 卡尔曼滤波: 相信所有学习卡尔曼滤波的同学首先接触的都是状态方程和观测方程,学过控制系统的同学可能不陌生,否则,先被那两个看起来好深奥的公式给吓跑了,关键是还不知道他们究竟是干什么的,什么是...
  • 卡尔曼滤波、粒子滤波、直方图滤波是贝叶斯滤波的三种实现形式,在《概率机器人》这本书中,按照“线性→非线性”的顺序讲解,先介绍卡尔曼滤波,再介绍直方图滤波和粒子滤波。但我发现先介绍直方图滤波效果可能会...
  • 实战低通滤波和卡尔曼滤波

    千次阅读 多人点赞 2020-05-09 10:25:44
    滤波的方法有太多了,从简单的均值滤波(其实我认为应该叫状态估计)、中值滤波到能把飞船送上月球的卡尔曼滤波。往往我们都会选择适合工程需求的滤波器对信号进行滤波。那么本次就从理论到实践,从公式推导到算法...
  •  前面博客介绍了贝叶斯滤波的详细推导,里面说了贝叶斯滤波的缺点是无穷积分没办法准确积分的问题。 缺点也很明显,从fk−1+(x)→fk−(x...卡尔曼滤波(KF) 、扩展卡尔曼滤波(EKF)、UKF、高斯积分滤波、粒子滤波(蒙特
  • 理解卡尔曼滤波

    千次阅读 2015-09-25 10:29:16
    卡尔曼滤波被广泛的应用于运动估计中,在飞行器中多有应用,区别于普通滤波如低通滤波,卡尔曼滤波具有不延时的优点,即普通的低通滤波在过滤噪声的同时也会引入信号滞后,而卡尔曼滤波则可以实时估计对象状态,不...

空空如也

1 2 3 4 5 ... 20
收藏数 36,533
精华内容 14,613
关键字:

滤波