卡尔曼滤波处理三轴加速度数据（MATLAB）

求上进的小怪兽

已于 2023-05-08 11:23:00 修改

阅读量1w

点赞数 59

文章标签： matlab 算法矩阵概率论

于 2022-12-29 16:52:08 首次发布

本文链接：https://blog.csdn.net/qq_50597556/article/details/128478744

版权

根据实际需要以及传感器数据列写状态方程，确定观测数据和相关误差。

附加一部分六轴传感器数据的处理技术总结，有问题欢迎指出。

文章目录

前言

之前获取各种检测传感器数据总是使用最简单的均值滤波，简单粗暴，但是略显粗糙；

之前了解过卡尔曼滤波，现在将近期的卡尔曼学习以及实践验证结果在此总结分享。

正文：

一、卡尔曼滤波的原理

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。 Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。

任何精度的传感器都不可能提供真实的数据，都有噪声的存在，而卡尔曼滤波的作用就是对真实值的最优化估计；对卡尔曼滤波过程通俗的理解就是：已知前一时刻的状态，对下一时刻的真实状态的预测估计和测量更新，实际过程是一个不断推理迭代的过程。

Kalman Filtering是通过调整观测值和估计值的权重，使修正后的值更趋向于相信观测值还是相信估计值的一个过程。

当前估计值 = 上一时刻估计值 + kalman增益 *（测量值 - 上一时刻估计值）

主要更新公式：

（一）预测更新（先验）

（1）先验估计： $\hat{x}_{k}^{-} =A \hat{x}_{k-1} + B u_{k-1}$

（2）先验误差协方差： $P_{k}^{-}=AP_{k-1}A^{T}+Q$

（二）测量更新（后验）

（3）计算卡尔曼增益： $K_{k}=\frac{P_{k}^{-}H^T }{HP_{k}^{-}H^T+R}$

（4）根据测量值更新估计值： $\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}(z_{k}-H\hat{x}_{k}^{-})$

（5）更新误差协方差矩阵： $P_{k}=(I-K_{k}H)P_{k}^{-}$

公式中， $\hat{x}_{k}$ 表示K时刻的估计值， $P_{k}^{-}$ 、 $\hat{x}_{k}^{-}$ 均表示先验值；

P表示误差协方差矩阵；误差协方差的初始值表示我们对当前预测状态的信任度，越小表示越相信模型预测结果；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。
H为系统观测矩阵，及对应系统状态 $X_{k}$ 与观测量 $Z_{k}$ 之间的转换关系；
Q表示过程噪声（系统噪声），一般为对角矩阵，系统动态响应速度会随着Q的增大而变快，收敛稳定性会变差；Q越小，代表对模型预测结果的信任度越高；
R表示量测噪声，系统动态响应速度会随着R的增大而变慢，收敛稳定性变好；R越小，代表对测量结果的信任度越高；
测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。
实践表明：固定估计噪声和量测噪声方差的比例一定时，滤波效果一致（卡尔曼滤波的实际等效模型为历史测量值的权重与最新测量值的权重求和，权重比例不变，结果必然不变）。

二、使用MATLAB验证滤波算法

1.记录及保存数据

代码如下：

clear all;
close all;
instrreset;
disp('Press Ctrl+C to stop collecting data!')              %按Ctrl+C,终止数据的获取
s=serial('com4','baudrate',9600) ;fopen(s) ;               %请将COM44换成电脑识别到的COM口，波特率9600换成传感器对应的波特率    Please replace COM44 with the COM port recognized by the PC, and change the baud rate 9600 to the baud rate corresponding to the sensor
f = 20;         %DataFrequce
t=0;
cnt = 1;
aa=[0 0 0];     %加速度XYZ    Acceleration X, Y, Z axis
ww=[0 0 0];     %角速度XYZ    Angular velocity X, Y, Z axis
AA = [0 0 0];   %角度XYZ      Angle X, Y, Z axis
tt = 0;
a=[0 0 0]';
w=[0 0 0]';
A=[0 0 0]';
while(1)
    Head = fread(s,2,'uint8');                              %获取串口数据，其中s文件，已经在上面提及到  Getting serial data, the S file has been mentioned above
    if (Head(1)~=uint8(85))                                 %如果串口的第一个数据不等于85(0x55)，证明不符合协议，不进行数据解析  If the first data of the serial is not equal to 85 (0x55), it proves that it isn't conform to the protocol and haven't perform data analysis
        continue;
    end   
    Head(2)
    switch(Head(2))                                         %获取串口第二个数据   Getting the second data of the serial
        case 81                                             %81(0x51):加速度包   81(0x51): Acceleration data packet
            a = fread(s,3,'int16')/32768*16;                %获取3个16bit的加速度数据，请参考协议说明   Getting three 16bit acceleration data, please refer to the protocol
        case 82                                             %82(0x52):角速度包   82 (0x52): Angular velocity data packet
            w = fread(s,3,'int16')/32768*2000;              %获取3个16bit的角速度数据，请参考协议说明   Getting three 16bit angular velocity data, please refer to the protocol
        case 83                                             %83(0x53):角度包     83 (0x53): Angular data packet
            A = fread(s,3,'int16')/32768*180;               %获取3个16bit的角度数据，请参考协议说明     Getting three 16bit angle data, please refer to the protocol.
            aa = [aa;a'];
            ww = [ww;w'];
            AA = [AA;A'];
            tt = [tt;t];
            
            subplot(3,1,1);plot(tt,aa);title(['Acceleration = ' num2str(a') 'm2/s']);ylabel('m2/s');
            subplot(3,1,2);plot(tt,ww);title(['Gyro = ' num2str(w') '°/s']);ylabel('°/s');
            subplot(3,1,3);plot(tt,AA);title(['Angle = ' num2str(A') '°']);ylabel('°');              
            cnt=0;
            drawnow;
%             if (size(aa,1)>5*f)                              %清空历史数据   Clear history data
%                 aa = aa(f:5*f,:);
%                 ww = ww(f:5*f,:);
%                 AA = AA(f:5*f,:);
%                 tt = tt(f:5*f,:);
%             end
            t=t+0.1;                                         %数据默认是10Hz，也就是0.1s，如果更改了产品的输出速率，请把0.1改为其他输出速率   The data default is 10Hz, which is 0.1s. If you change the output rate of the product, please change 0.1 to other output rates
    end 
        
            End = fread(s,3,'uint8');
end
fclose(s);

写入文件：


data = aa;  %合成数据
fid = fopen('step_acc_data_00.txt','w');
for i = 1 : length(data)
    fprintf(fid,'%10g    ',data(i,1)); 
    fprintf(fid,'%10g    ',data(i,2)); 
    fprintf(fid,'%10g    ',data(i,3)); 
    fprintf(fid,'%10g  \n',tt(i,1));
end
fclose(fid);

data = AA;  %合成数据
fid = fopen('step_angle_data_00.txt','w');
for i = 1 : length(data)
    fprintf(fid,'%10g    ',data(i,1)); 
    fprintf(fid,'%10g    ',data(i,2)); 
    fprintf(fid,'%10g    ',data(i,3)); 
    fprintf(fid,'%10g  \n',tt(i,1));
end
fclose(fid);

2.三轴加速度去除重力

代码如下（需要根据实际坐标轴情况进行调整）：

X轴加速度 : $a_{xg}=a_{x}+0.98*sin(y)$ ;

Y轴加速度 = $a_{yg}= a_{y}-0.98*sin(x)*cos(y)$ ;

Z轴加速度 = $a_{zg}= a_{z}-0.98*cos(x)*cos(y)$ ;

注：三角函数中的x,y,z分别为绕三轴的旋转角（pitch，yaw，roll），单位为弧度制；

    data_ag_x(i)=  acc(i,1) + 0.988*sin(data_angle_y(i));  %重力加速度为0.988，估算为1
    data_ag_y(i)=  acc(i,2) - 0.988*sin(data_angle_x(i))*cos(data_angle_y(i));
    data_ag_z(i)=  acc(i,3) - 0.988*cos(data_angle_x(i))*cos(data_angle_y(i));

3.滤波算法主体

function [position,speed,acc]=karman_filter(data,step)
%%卡尔曼滤波函数实现：data为单轴加速度原始数据，step为数据步长；
    position=zeros(1,step);
    speed=zeros(1,step);
    acc=zeros(1,step);
    dtt = 0.1;                   %数据时间间隔（积分项）

    A= [1,dtt,0.5*dtt*dtt;
        0,1,dtt;
        0,0,0];                  %离散化后的状态阵
    
    H=[0,0,1];                   %观测阵 
        
    x=[0 0 0]';                  %状态变量为[位置，速度，加速度]；
    Q= [0.01, 0,    0;
        0,    0.01, 0;
        0,    0,    0.01];       %系统噪声 

    R=[0.1];                     %量测噪声
    Pk=[0.01,0,0;
        0,0.01,0;
        0,0,0.01];               %协方差矩阵
    
    for i=1:step                 %kalman更新公式
        x_c=[0,0,data(0)]';      %加速度测量值，用于递推离散加速度
        x_ = A * x + x_c;     %(1)
        Pk_ = A * Pk * A' + Q ;   %(2)
        Kk = (Pk_ * H')/(H * Pk_ * H' + R);   %(3)
        x = x_ + Kk * (data(i)- H*x_);         %(4)  
        Pk = (diag([1 1 1]) - Kk * H) * Pk_;  %(5)
    
        position(i) = x(1);
        speed(i) = x(2);
        acc(i) = x(3);
    end
    
end

三、结果验证

%% 绘图
figure(1)
plot([1:step]*dtt,acc_x,'LineWidth',2)  %估计加速度
hold on
plot([1:step]*dtt,data_a_x,'LineWidth',1)  %原始数据
legend('估计X轴加速度','原始数据')
title('X轴加速度')
xlabel('时间(s)')
ylabel('加速度(m/s^2)')

figure(2)
plot([1:step]*dtt,acc_y,'LineWidth',2)  %估计加速度
hold on
plot([1:step]*dtt,data_a_y,'LineWidth',1)  %原始数据
legend('估计Y轴加速度','原始数据')
title('Y轴加速度')
xlabel('时间(s)')
ylabel('加速度(m/s^2)')

figure(3)
plot([1:step]*dtt,acc_z,'LineWidth',2)  %估计加速度
hold on
plot([1:step]*dtt,data_a_z,'LineWidth',1)  %原始数据
legend('估计Z轴加速度','原始数据')
title('Z轴加速度')
xlabel('时间(s)')
ylabel('加速度(m/s^2)')

以下数据为正常走路时的加速度实测数据：

四、计步算法初步实现

以X轴为计算实例：通过加速度曲线过零检测与峰值检测来确定步频，检测到的符合阈值条件的峰值个数即为最终步数；下一步打算使用步数*步长实现基本的航迹推算，计步代码实现：

function num_slide = detect_slide(data_x,step)  %data_x为检测轴加速度数据，step为步长
%步数检测函数
    a_max=max(data_x);

    a_thread=a_max/4;    %根据实际情况调整，目前合适阈值为1/4*max
    step_cnt=0;

    ax_new=0;
    for i=1:step
        ax_old=ax_new;
        if abs(data_x(i)-ax_old)>a_thread
            ax_new=data_x(i);     
            if(ax_old>0 && ax_new<0) %加速度过零检测+峰值（阈值）检测
                step_cnt=step_cnt+1;
            end            
        end
    end
    num_slide=step_cnt;

end

实现效果：

总结

卡尔曼滤波原理
MATLAB实际应用
验证效果

现存问题：

1. 对于卡尔曼滤波更新公式中的误差及协方差矩阵的确定，其相关参数的确定在很大程度上影响了滤波结果，容易导致结果发散（误差累计）。

2. 需要继续学习怎样估计噪声的协方差，使其与实际的系统参数匹配达到更好的效果。

3. 部分内容参考B站up主（DR_CAN）的视频讲解以及CSDN上的相关博客，致谢：

[1]【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili

[2] (10条消息) 卡尔曼滤波（Kalman Filtering）——（6）MATLAB仿真（保姆级）_@曾记否的博客-CSDN博客_卡尔曼滤波流程图

[3](10条消息) 详解卡尔曼滤波原理_engineerlixl的博客-CSDN博客_卡尔曼滤波

[4]【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)_哔哩哔哩_bilibili\

[5] (10条消息) 卡尔曼滤波——包含C/C++实现方式及MPU6050实例_古城凉梦的博客-CSDN博客_卡尔曼滤波c++