找回密码
 立即注册

QQ登录

只需一步,快速开始

扫一扫,访问微社区

查看: 5582|回复: 15

飞行器姿态及速度估计的卡尔曼滤波器C语言实现

[复制链接]
发表于 2018-3-16 15:41:27 | 显示全部楼层 |阅读模式
本帖最后由 Bluesky 于 2018-5-10 23:53 编辑

作者:BlueSky

随着多旋翼飞行器的技术进一步成熟,越来越多的人开始参与到多旋翼飞控的学习与开发中来,在这个过程中,大部分新手碰到的一个拦路虎便是大名鼎鼎的卡尔曼滤波器。
鉴于网上关于卡尔曼滤波器的说明与教程已经数不胜数,本文不过多地讲述卡尔曼的过去与将来,只以一个简单的飞行器速度估计实例来讲解卡尔曼滤波器的实现过程(姿态估计在4楼)。


需求描述:
当一个多旋翼在空中飞行时,我们需要准确计算出该飞行器当前在地理坐标系下的三轴飞行速度,以便能控制飞行器匀速飞行。而飞控上的加速度传感器、GPS、气压计均能够通过某种方式测量得到速度信息:
    其中加速度传感器能够测量当前机体坐标系下的运动加速度,转换到地理坐标系后,积分得到速度。
    GPS可以测量得到飞行器在地球坐标系下的水平XY速度(Z轴垂直速度也能测出但误差太大不考虑使用)。
    气压计能够测出当前飞行器的气压高度,经过微分便能得到垂直方向的速度。

但是这些直接由传感器计算得到的速度数据,存在着各自的优点和缺陷:
    加速度积分的优点是实时性最高(通常1KHz甚至更高),数据平滑无跳变,而缺点也很显而易见,由于加速度测量值本身存在误差,经过积分后这些误差逐渐累加,使得加速度积分得到的速度值会逐渐偏离真实值。
    GPS速度的优点是长期稳定可靠,缺点是更新频率低(通常10Hz)实时性差,且GPS定位效果受环境影响(比如存在遮挡),误差不稳定。
    气压速度的优点一样是长期比较稳定,缺点是更新频率稍低(通常50Hz),且数据噪声过大。

综上所述,我们需要一种方法,把以上数据综合利用,得到一个最为接近真实状态的飞行速度值,这就用到了卡尔曼滤波器(Kalman Filter)。卡尔曼并不是传统的高低通频率滤波器,而是一种最优估计方法。在这个例子中,我们需要利用多个数据得到飞行器速度的最优估计值。

卡尔曼的工作流程:
网上的这张图片基本能说明卡尔曼滤波器的整个工作流程
QQ截图20180316150630.jpg


卡尔曼的流程分解:

1.状态预估计
QQ截图20180316162337.jpg

其中
QQ截图20180316163816.jpg
为飞行器的三轴速度估计值 QQ截图20180316163816.jpg
为状态转移矩阵 QQ截图20180316163816.jpg
为控制输入矩阵,t为卡尔曼滤波器的运行周期,本例中为0.001S
QQ截图20180316163816.jpg
为飞行器在地理坐标系下的三轴运动加速度

2.误差协方差矩阵预更新
QQ截图20180316162345.jpg

P为误差协方差矩阵,初始值为0

Q为过程噪声协方差矩阵,这里设为 QQ截图20180316163816.jpg


3.计算误差矩阵
QQ截图20180316162714.jpg

QQ截图20180316163816.jpg
为误差矩阵 QQ截图20180316165944.jpg
为观测矩阵 QQ截图20180316170100.jpg
为观测映射矩阵


4.计算卡尔曼增益
QQ截图20180316162732.jpg

R为测量噪声协方差矩阵,这里设为 QQ截图20180316163816.jpg


5.修正当前状态
QQ截图20180316162742.jpg


6.更新协方差矩阵
QQ截图20180316162749.jpg

这一步有人提出疑问,故附上一张某文档截图加以说明 20180319103347.jpg


7.回到第一步,完成递归

卡尔曼的C语言实现:
下面是上述例子中主要步骤的C语言实现,完整代码可以下载附件查看。写的比较简陋,仅为新手学习参考使用。 kalman.rar (2.01 KB, 下载次数: 47, 售价: 5 金钱)

[C] 纯文本查看 复制代码
//飞行器速度估计值,为方便计算写成3阶矩阵的形式
float velocity[9] = {0, 0, 0,
                     0, 0, 0,
                     0, 0, 0};
//飞行器在地理系上的加速度值,为方便计算写成3阶矩阵的形式
float accel[9] = {0, 0, 0,
                 0, 0, 0,
                 0, 0, 0};
//飞行器的速度观测值,为方便计算写成3阶矩阵的形式
float Z[9] = {0, 0, 0,
           0, 0, 0,
           0, 0, 0};
//误差矩阵,为方便计算写成3阶矩阵的形式
float Y[9] = {0, 0, 0,
           0, 0, 0,
           0, 0, 0};
//状态转移矩阵
float F[9] = {1, 0, 0,
              0, 1, 0,
              0, 0, 1};
//状态转移矩阵的转置
float F_t[9] = {1, 0, 0,
              0, 1, 0,
              0, 0, 1};
//观测映射矩阵
float H[9] = {1, 0, 0,
               0, 1, 0,
               0, 0, 1};
//观测映射矩阵的转置
float H_t[9] = {1, 0, 0,
                0, 1, 0,
                0, 0, 1};
//过程矩阵,存放计算结果
float S[9] = {0, 0, 0,
                 0, 0, 0,
                 0, 0, 0};
//卡尔曼增益矩阵
float K[9] = {0, 0, 0,
                  0, 0, 0,
                  0, 0, 0};
//控制输入矩阵*deltaT deltaT=0.001S
float B[9] = {0.001, 0,  0,
                   0, 0.001, 0,
                   0, 0, 0.001};
//误差协方差矩阵
float P[9] = {0, 0, 0,
                   0, 0, 0,
                   0, 0, 0};
//单位矩阵
float I[9] =  {1, 0, 0,
                    0, 1, 0,
                    0, 0, 1};
//过程噪声协方差矩阵
float Q[9] = {0.005, 0, 0,
                    0, 0.005, 0,
                    0, 0, 0.001};
//测量噪声协方差矩阵
float R[9] = {100, 0,  0,
                    0, 100,  0,
                    0, 0, 3600};

//运行频率1KHz
//卡尔曼滤波,输入量为飞行器在地理坐标系下的三轴运动加速度、水平方向的GPS速度测量值
//以及垂直方向的速度测量值,这个由气压高度测量值微分得来(数据噪声很大)
void KalmanFilter(float accEf[3], float gpsVelBf[2], float baroVel)
{
        //用于存放计算结果的临时矩阵
        float m1[9], m2[9],  m3[9],  m4[9],  m5[9];
        
        //更新加速度矩阵
        accel[0] = accEf[0];
        accel[3] = accEf[1];        
        accel[6] = accEf[2];
        //更新速度观测矩阵
        Z[0] = gpsVelBf[0];
        Z[3] = gpsVelBf[1];
        Z[6] = baroVel;
        
        //1:状态预估计 Xk = Fk*Xk-1 + Bk*Uk
        Matrix3_Mul(F, velocity, m1);
        Matrix3_Mul(B, accel, m2);   
        Matrix3_Add(m1, m2, velocity);
        //2:误差协方差矩阵预更新 Pk = Fk*Pk-1*FkT + Qk
        Matrix3_Mul(F, P, m1);
        Matrix3_Mul(m1, F_t, m2);    
        Matrix3_Add(m2, Q, P);
         //3:计算误差矩阵 Yk = Zk - Hk*Xk
        Matrix3_Mul(H, velocity, m1);
        Matrix3_Sub(Z, m1, Y);
        //4:Sk = Hk*Pk*HkT + Rk
        Matrix3_Mul(H, P, m1);
        Matrix3_Mul(m1, H_t, m2);
        Matrix3_Add(m2, R, S);   
        //5:计算卡尔曼增益 Kk = Pk*HkT*Sk-1
        Matrix3_Det(S, m1);
        Matrix3_Mul(P, H_t, m2);
        Matrix3_Mul(m2, m1, K);
        //6:修正当前状态 Xk = Xk + Kk*Yk
        Matrix3_Mul(K, Y, m1);
        Matrix3_Add(velocity, m1, m2); 
        Matrix3_Copy(m2, velocity);
        //7:更新协方差矩阵 Pk = (I-Kk*Hk)*Pk*(I-Kk*Hk)T + Kk*Rk*KkT
        Matrix3_Mul(K, H, m1);
        Matrix3_Sub(I, m1, m2);
        Matrix3_Tran(m2, m3);
        Matrix3_Mul(m2, P, m4);
        Matrix3_Mul(m4, m3, m5);
        Matrix3_Mul(K, R, m1);
        Matrix3_Tran(K, m2);
        Matrix3_Mul(m1, m2, m3);
        Matrix3_Add(m5, m3, P);
}

运行结果:
以z轴垂直速度为例(在室内接收不到GPS信号),把小飞机放在手里上下运动,得到垂直速度变化的一个波形图

其中黄线是气压速度波形,蓝线为速度估计值波形(卡尔曼滤波后)
QQ截图20180316192849.jpg



Q、R噪声协方差矩阵的确定:

看完上述例子,可能有人会问:如何确定卡尔曼滤波器中的Q和R矩阵?
我的答案是理论加调试。

理论上,Q、R矩阵为协方差矩阵,我们假设三个轴数据的噪声是相互独立的,那么就只剩下自己和自己的协方差,也就是方差,如:
9.jpg


以z轴速度观测值即气压速度噪声为例来计算噪声方差:将飞行器静止放置,采集一段时间内的气压速度数据,如下图
QQ截图20180316134224.jpg


将采集到的数据放入excel,绘制直方图:
QQ截图20180316194139.jpg
    QQ截图20180316150704.jpg


从直方图可以直观看出气压速度噪声数据服从高斯分布,并由excel计算得到 10.jpg


因此得到气压速度的噪声方差

而过程噪声方差矩阵Q的确定较为麻烦,其包含了模型误差,数值计算精度误差,离散化数值积分误差,加速度传感器数据噪声误差等。
但通常来说我们倾向于更多地相信加速度积分的准确性(尽管有些时候可能存在着较大的漂移误差),所以尽可能将Q值设置小一点,在R矩阵已经确定的情况下,可以通过调试来确定一个比较合适的Q矩阵。

未完待续


回复

使用道具 举报

 楼主| 发表于 2018-3-16 19:46:44 | 显示全部楼层
本帖最后由 Bluesky 于 2018-3-17 00:03 编辑

透过现象看本质 我觉得这句话才是真正工程应用中的精髓,新手在学习传感器融合算法时,总会错误地把重点放在工具上,你可以在搜索引擎上可以搜到许多“ CF KF EKF UKF 哪种滤波器效果最好”之类的问题。
但实际上,想要做到理想的融合效果,估计出飞行器最真实的状态,我们所需要考虑的,应该是真实世界中会对测量数据造成各种影响的因素,然后建立一个最接近真实物理世界的数学模型,以造物主的角度来还原出一个真实的世界。

上面说了一堆废话,还是回到一楼的卡尔曼滤波例程。
这个例子里,我们建立了一个较为简单的状态空间模型,以及一个从假设出发的理想化噪声协方差矩阵。事实上这真的会得到很好的效果么,从滤波器运行的情况来看,效果的确还马马虎虎,但是在实际飞行中,我们可能会碰到很多麻烦。比如:我们假设气压计速度噪声是服从高斯分布的,当我们静止放置飞行器,采集气压计数据,从结果来看也的确如此。然而当飞机在实际工作时,情况又不一样了,环境的风会使气压上下波动,增加了一个随机误差,与此同时,飞机会以一定的速度飞行,造成机身内外存在一个随速度变化的气压差。这些问题就不是简单的一个高斯分布可以描述得了了。

但是,这些并不是最致命的,对于一个惯性导航系统来说,加速度积分才是重点中的重点,实际工程应用中大部分问题来源于此(原因在于相对其它传感器测量信息只有加速度积分才是最靠谱的以至于我们不得不尽可能相信它)。

我们使用地理坐标系下的加速度值用于积分,而这个加速度值存在哪些误差,我可以大概描述一下(可能不是非常全面,以后慢慢补充):
1.加速度传感器本身存在制造误差,这个可以通过椭球拟合标定得到零偏校准和刻度校准参数;
2.环境温度变化带来的传感器ADC温漂误差,可加一个发热电阻实现传感器恒温,目前产品级多旋翼飞控标配;
3.加速度传感器和飞行器不在同一个坐标系上(安装误差),这个可以通过水平校准得到一个误差旋转矩阵;
4.加速度传感器没有被安装在飞行器的中心点上,这样当飞机绕中心点做旋转运动时会产生一个向心加速度误差,这个采集数据后可以通过最小二乘法拟合出误差参数;
5.即使加速度传感器被安装在飞行器的中心点上,当飞行器做圆周飞行运动时,还是会产生一个向心加速度误差,一样要想办法补偿;
6.电机和桨叶工作时产生的震动带来的加速度误差,通常会给加速度传感器做一定的减震措施,使得震动频率在可控范围内(不超出传感器的采样频率),这个情况下震动噪声也基本是服从高斯分布的。极端情况如电机和桨叶的部分损坏(依然能工作)带来的强烈震动则需要额外考虑了;
7.最后是大头,姿态解算误差导致的加速度坐标转换误差,姿态解算如何产生误差,可以下面再详细说明。但可以确定的是,姿态误差的存在会给加速度转换带来极大的误差,而且这个误差是非线性的(因为在这一层看来姿态误差是个无法确定的非线性函数)。而由于重力加速度的方向性以及三角函数特性,姿态误差带来的加速度误差在水平XY轴上要远大于Z轴。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-3-16 19:46:52 | 显示全部楼层
本帖最后由 Bluesky 于 2018-3-26 09:33 编辑

接下来分析一下姿态误差的主要来源

对于惯导而言,姿态解算是最基本的一步,且产生的误差会在后面的运算环节被不断累积放大,因此一个优秀的惯性导航系统要求要有一个极其高精度的姿态解算。
在计算姿态时,陀螺仪传感器占了很大的比重,由于陀螺仪数据相对而言误差更小,也更好处理,就不过多的说明了。最大的误差来源,来自于飞行器的运动加速度。好在这个是一个可确定的误差函数,因此我们建立状态空间模型时,应考虑加入运动加速度误差函数。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-3-19 08:40:04 | 显示全部楼层
本帖最后由 Bluesky 于 2018-5-10 23:53 编辑

这里再写一个以飞行器姿态估计为例的卡尔曼滤波器

和速度估计的例子基本相似,只改变了状态量和状态更新方程。
这里我们去估计的状态是一个三维向量X,表示重力加速度在机体坐标系上的投影,意义上等同于加速度传感器测量值,但这里它是一个估计量。

其状态空间方程为: 20180319112616.jpg


F为状态转移矩阵,这里为某时刻内的角度变换量构建的方向余弦矩阵

下面是代码

[C] 纯文本查看 复制代码
//飞行器姿态估计值,为方便计算写成3阶矩阵的形式
static float attitude[9] = {0, 0, 0,
                                                                                 0, 0, 0,
                                                                                 0, 0, 0};

//飞行器的姿态观测值,为方便计算写成3阶矩阵的形式
static float Z[9] = {0, 0, 0,
                                                        0, 0, 0,
                                                        0, 0, 0};
//误差矩阵,为方便计算写成3阶矩阵的形式
static float Y[9] = {0, 0, 0,
                                                        0, 0, 0,
                                                        0, 0, 0};
//状态转移矩阵
static float F[9] = {1, 0, 0,
                                                        0, 1, 0,
                                                        0, 0, 1};
//状态转移矩阵的转置
static float F_t[9] = {1, 0, 0,
                                                                0, 1, 0,
                                                                0, 0, 1};
//观测映射矩阵
static float H[9] = {1, 0, 0,
                                                        0, 1, 0,
                                                        0, 0, 1};
//观测映射矩阵的转置
static float H_t[9] = {1, 0, 0,
                                                                0, 1, 0,
                                                                0, 0, 1};
//过程矩阵,存放计算结果
static float S[9] = {0, 0, 0,
                                                        0, 0, 0,
                                                        0, 0, 0};
//卡尔曼增益矩阵
static float K[9] = {0, 0, 0,
                                                        0, 0, 0,
                                                        0, 0, 0};
//误差协方差矩阵
static float P[9] = {0, 0, 0,
                                                        0, 0, 0,
                                                        0, 0, 0};
//单位矩阵
static float I[9] =  {1, 0, 0,
                                                         0, 1, 0,
                                                         0, 0, 1};
//过程噪声协方差矩阵
static float Q[9] = {0.0003, 0, 0,
                                                        0, 0.0003, 0,
                                                        0, 0, 0.0003};
//测量噪声协方差矩阵
static float R[9] = {1500, 0,  0,
                                                        0, 1500,  0,
                                                        0, 0, 1500};


void F_Update(float F[9], float deltaGryo[3])
{
        float cosx, sinx, cosy, siny, cosz, sinz;
        float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;

        cosx = cosf(deltaGryo[0]);
        sinx = sinf(deltaGryo[0]);
        cosy = cosf(deltaGryo[1]);
        siny = sinf(deltaGryo[1]);
        cosz = cosf(deltaGryo[2]);
        sinz = sinf(deltaGryo[2]);

        coszcosx = cosz * cosx;
        coszcosy = cosz * cosy;
        sinzcosx = sinz * cosx;
        coszsinx = sinx * cosz;
        sinzsinx = sinx * sinz;

        F[0] = coszcosy;
        F[1] = sinzcosx + (coszsinx * siny);
        F[2] = (sinzsinx) - (coszcosx * siny);
        F[3] = -cosy * sinz;
        F[4] = coszcosx - (sinzsinx * siny);
        F[5] = (coszsinx) + (sinzcosx * siny);
        F[6] = siny;
        F[7] = -sinx * cosy;
        F[8] = cosy * cosx;
}

//运行频率1KHz
//卡尔曼滤波,输入量为陀螺仪角度变化量、加速度传感器测量值(不用关心单位)
//deltaAngle = gyro * deltaT,单位为弧度
void KalmanFilter(float deltaAngle[3], float acc[3])
{        
        //用于存放计算结果的临时矩阵
        float m1[9], m2[9],  m3[9],  m4[9],  m5[9];
        
        static float angle[2];
        
        //更新姿态观测矩阵
        Z[0] = acc[0];
        Z[3] = acc[1];
        Z[6] = acc[2];
        
        //更新状态转移矩阵
        F_Update(F, deltaAngle);
        Matrix3_Tran(F, F_t);
                
        //1:状态预估计 Xk = Fk*Xk-1
        Matrix3_Mul(F, attitude, m1);  
        Matrix3_Copy(m1, attitude);
        //2:误差协方差矩阵预更新 Pk = Fk*Pk-1*FkT + Qk
        Matrix3_Mul(F, P, m1);
        Matrix3_Mul(m1, F_t, m2);    
        Matrix3_Add(m2, Q, P);
         //3:计算误差矩阵 Yk = Zk - Hk*Xk
        Matrix3_Mul(H, attitude, m1);
        Matrix3_Sub(Z, m1, Y);
        //4:Sk = Hk*Pk*HkT + Rk
        Matrix3_Mul(H, P, m1);
        Matrix3_Mul(m1, H_t, m2);
        Matrix3_Add(m2, R, S);   
        //5:计算卡尔曼增益 Kk = Pk*HkT*Sk-1
        Matrix3_Det(S, m1);
        Matrix3_Mul(P, H_t, m2);
        Matrix3_Mul(m2, m1, K);
        //6:修正当前状态 Xk = Xk + Kk*Yk
        Matrix3_Mul(K, Y, m1);
        Matrix3_Add(attitude, m1, m2); 
        Matrix3_Copy(m2, attitude);
        //7:更新协方差矩阵 Pk = (I-Kk*Hk)*Pk*(I-Kk*Hk)T + Kk*Rk*KkT
        Matrix3_Mul(K, H, m1);
        Matrix3_Sub(I, m1, m2);
        Matrix3_Tran(m2, m3);
        Matrix3_Mul(m2, P, m4);
        Matrix3_Mul(m4, m3, m5);
        Matrix3_Mul(K, R, m1);
        Matrix3_Tran(K, m2);
        Matrix3_Mul(m1, m2, m3);
        Matrix3_Add(m5, m3, P);
        
        //转换成欧拉角
        angle[ROLL] = degrees(atan2f(attitude[3], sqrtf(attitude[0] * attitude[0] + attitude[6] * attitude[6])));
        angle[PITCH] = degrees(atan2f(-attitude[0], attitude[6]));
}




看完你可能会有疑问:还有一个YAW如何计算?
上述例子中的状态量只能描述ROLL和PITCH,如果需要计算YAW,那我们需要用另外一个状态量来表示,其意义为地磁场向量在机体坐标系上的投影。计算形式基本一样,唯一一个区别是最后转成航向角时,我们需要将该向量转换回地理坐标系。


如何直接用一组状态量来描述飞行器的三轴姿态?
    欧拉角(这个基本不考虑用于过程计算)
    方向余弦矩阵。包含了9个量,会使得计算量和复杂度大大增加
    四元数。四元数只有4个量,方便计算,基本上我们只能选择使用这个。








回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-3-19 11:08:03 | 显示全部楼层
占楼+2222222222222222222222222222
回复 支持 反对

使用道具 举报

发表于 2018-3-29 14:31:18 | 显示全部楼层
66666
大佬什么时候出一份关于扩展卡尔曼的教程啊
回复 支持 反对

使用道具 举报

发表于 2018-4-3 13:47:14 | 显示全部楼层
谢谢楼主分享
回复 支持 反对

使用道具 举报

发表于 2018-4-25 20:44:30 | 显示全部楼层
学习一下,谢谢
回复 支持 反对

使用道具 举报

发表于 2018-5-3 11:07:24 | 显示全部楼层
谢谢分享,学习了
回复 支持 反对

使用道具 举报

发表于 2018-5-4 14:28:31 | 显示全部楼层
谢谢大佬
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

快速回复 返回顶部 返回列表