卡尔曼滤波处理三轴加速度数据（MATLAB）
根据实际需要以及传感器数据列写状态方程，确定观测数据和相关误差。
附加一部分六轴传感器数据的处理技术总结，有问题欢迎指出。
文章目录
一、卡尔曼滤波的原理
二、使用MATLAB验证滤波算法
三、结果验证
四、计步算法初步实现
总结
前言
之前获取各种检测传感器数据总是使用最简单的均值滤波，简单粗暴，但是略显粗糙；
之前了解过卡尔曼滤波，现在将近期的卡尔曼学习以及实践验证结果在此总结分享。
正文：
一、卡尔曼滤波的原理
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。 Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。
任何精度的传感器都不可能提供真实的数据，都有噪声的存在，而卡尔曼滤波的作用就是对真实值的最优化估计； 对卡尔曼滤波过程通俗的理解就是：已知前一时刻的状态，对下一时刻的真实状态的预测估计和测量更新，实际过程是一个不断推理迭代的过程。
Kalman Filtering是通过调整观测值和估计值的权重，使修正后的值更趋向于相信观测值还是相信估计值的一个过程。
当前估计值 = 上一时刻估计值 + kalman增益 *（测量值 - 上一时刻估计值）
主要更新公式：
（一）预测更新（先验）
（1）先验估计：
（2）先验误差协方差：
（二）测量更新（后验）
（3）计算卡尔曼增益：
（4）根据测量值更新估计值：
（5）更新误差协方差矩阵：
公式中，表示K时刻的估计值，
、
均表示先验值；
- P表示误差协方差矩阵；误差协方差的初始值表示我们对当前预测状态的信任度，越小表示越相信模型预测结果；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。
- H为系统观测矩阵，及对应系统状态
与观测量
之间的转换关系；
- 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 aboveif (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 analysiscontinue;end Head(2)switch(Head(2)) %获取串口第二个数据 Getting the second data of the serialcase 81 %81(0x51):加速度包 81(0x51): Acceleration data packeta = fread(s,3,'int16')/32768*16; %获取3个16bit的加速度数据，请参考协议说明 Getting three 16bit acceleration data, please refer to the protocolcase 82 %82(0x52):角速度包 82 (0x52): Angular velocity data packetw = fread(s,3,'int16')/32768*2000; %获取3个16bit的角速度数据，请参考协议说明 Getting three 16bit angular velocity data, please refer to the protocolcase 83 %83(0x53):角度包 83 (0x53): Angular data packetA = 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,:);
% endt=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 ratesend 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轴加速度 :
;
Y轴加速度 =
;
Z轴加速度 =
;
注：三角函数中的x,y,z分别为绕三轴的旋转角（pitch，yaw，roll），单位为弧度制；
data_ag_x(i)= acc(i,1) + 0.988*sin(data_angle_y(i)); %重力加速度为0.988，估算为1data_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);endend三、结果验证
%% 绘图
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*maxstep_cnt=0;ax_new=0;for i=1:stepax_old=ax_new;if abs(data_x(i)-ax_old)>a_threadax_new=data_x(i); if(ax_old>0 && ax_new<0) %加速度过零检测+峰值（阈值）检测step_cnt=step_cnt+1;end endendnum_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++
卡尔曼滤波处理三轴加速度数据（MATLAB）相关推荐
- mpu6050三轴加速度数据，三轴角速度数据显示
一.实验效果 显示mpu6050三轴加速度原始数据,三轴角速度原始数据. 硬件:stm32f427vit6单片机 .mpu6050模块 软件:STM32CubeIDE 1.8.0 C语言 二.硬件连接 ...
- SC7A20获取三轴加速度值
SC7A20获取三轴加速度值(I2C) 1. I2C.h #ifndef __I2C_H #define __I2C_H#include "config.h"sbit I2C_SC ...
- ADI Blackfin DSP处理器-BF533的开发详解59：DSP控制ADXL345三轴加速度传感器的应用2（含源码）
硬件准备 ADSP-EDU-BF533:BF533开发板 AD-HP530ICE:ADI DSP仿真器 软件准备 Visual DSP++软件 硬件链接 MEMS三轴加速度传感器 我做了一个三轴加速度 ...
- ADI Blackfin DSP处理器-BF533的开发详解58：DSP控制ADXL345三轴加速度传感器的应用（含源码）
硬件准备 ADSP-EDU-BF533:BF533开发板 AD-HP530ICE:ADI DSP仿真器 软件准备 Visual DSP++软件 硬件链接 MEMS三轴加速度传感器 我做了一个三轴加速度 ...
- ADI Blackfin DSP处理器-BF533的开发详解60：DSP控制ADXL345三轴加速度传感器-电子水平仪（含源码）
硬件准备 ADSP-EDU-BF533:BF533开发板 AD-HP530ICE:ADI DSP仿真器 软件准备 Visual DSP++软件 硬件链接 MEMS三轴加速度传感器 我做了一个三轴加速度 ...
- ADI Blackfin DSP处理器-BF533的开发详解61：DSP控制ADXL345三轴加速度传感器-LCD（含源码）
硬件准备 ADSP-EDU-BF533:BF533开发板 AD-HP530ICE:ADI DSP仿真器 软件准备 Visual DSP++软件 硬件链接 MEMS三轴加速度传感器 我做了一个三轴加速度 ...
- ADI Blackfin DSP处理器-BF533的开发详解62：DSP控制ADXL345三轴加速度传感器-贪食蛇游戏（含源码）
硬件准备 ADSP-EDU-BF533:BF533开发板 AD-HP530ICE:ADI DSP仿真器 软件准备 Visual DSP++软件 硬件链接 MEMS三轴加速度传感器 我做了一个三轴加速度 ...
- 三轴加速度传感器BMA250解读
BMA250凭借其超小的封装,低功耗越来越多的产品在这个芯片作为辅助,比如GPS定位时防止静止时定位信息漂移,可以借助其判断物体是静止还是运动.还有实现计步功能.角度测试等------ 经过几天对BM ...
- 三轴加速度传感器bma150驱动解析
BMA150 博世 三轴加速度传感器 SPI(4线,3线),i2c,中断引脚 频响+/- 2g,4g,8g;带宽25~1500hz,中断触发内部加速度求值 低功耗,快速唤醒 包含数据寄存器,控制寄存器 ...
最新文章
- r语言siggenes包_初探R语言可视化交互式包plotly——旭日图（Sunburst Chart）
- 后端常用开源组件合集(持续更新中)
- bootstrap table 分组_bootstrap-table组合表头的实现方法
- Python实现字符串反转的几种方法
- windows 虚拟地址映射到物理地址
- 雷达的工作原理示意图_电磁阀的构成和工作原理示意图
- python有多少种模块_python如何查看有哪些模块
- 排序数字英文字母交错,由小到大
- python学习中软件开发知识点_Python 学习知识点总结归纳
- STM32工作笔记0057---外部中断实验
- 整体二分——[Poi2011]Meteors
- java opencv 人脸相似度_java+opencv实现人脸识别程序记录
- 日常运维小知识--1
- 微信开发者工具整个是个浏览器
- 【听课笔记】复旦大学遗传学_10肿瘤遗传学
- BUUCTF——CRYPTO（记录不熟悉的题）(4)
- 130行Python代码模仿“蚂蚁呀嘿”特效，太魔性了！
- 输入一个整数n，按要求生成一个n*n的蛇形矩阵
- 计算机网络之面试常考
- 【小萝莉说Crash】第二期：Unrecognized selector xxx 之 ForwardInvocation