三阶卡尔曼计算加速度、速度、高度的MATLAB仿真及STM32验证

本资源提供三阶卡尔曼算法对加速度计和气压计的采样值进行数据融合与滤波，得到加速度、速度、高度值 本次修复了速度为0的bug 详细资料可参照博客https://blog.csdn.net/qq_31847339/article/details/90040387

clear;
load（‘DataUp.mat‘）;
len = length（z）;
% 减去重力加速度后的加速度
a = （z-1）*9.8;

% 参数设置
sam_frq = 1000;
T = 1/sam_frq;
k = 0.09;%压高系数

% 观测误差R、过程误差Q
R = 0.5*eye（2）;
R（11） = 150;    
R（22） = 0.5;
Q = 0.0001*eye（3）;
Q（22） = 0.00001;

% 状态方程矩阵
F = [1T0.5*T*T;01T;001];
H = [-1/k00;001];
V = [pre（1）;0];

% 数据初始化
Xkf = zeros（3len）;
Z = zeros（2len）;
Z（1:） = pre;
Z（2:） = a;
Xkf（:1） = [0;0;0];
P0 = eye（3）;
P0（11） = 10;

% 卡尔曼滤波
for i = 2:len
    Xn = F*Xkf（:i-1）;
    P1 = F*P0*F‘+ Q;
    K = （P1*H‘）/（H*P1*H‘+R）;
    Xkf（:i） = Xn + K*（Z（:i）-H*Xn-V）;
    P0 = （eye（3）-K*H）*P1;
end

%显示图像
figure;
plot（（pre（1）-pre）*0.09）;
hold on;
plot（Xkf（1:））;
hold on;
plot（Xkf（2:））;
hold on;
plot（Xkf（3:））;
hold on;
plot（a）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-06-12 09:08  三阶卡尔曼仿真及验证\
     目录           0  2019-05-10 14:51  三阶卡尔曼仿真及验证\Matlab仿真\
     文件     1466510  2019-05-10 13:56  三阶卡尔曼仿真及验证\Matlab仿真\DataDown.mat
     文件     1368417  2019-05-10 13:56  三阶卡尔曼仿真及验证\Matlab仿真\DataUp.mat
     文件         824  2019-05-10 13:59  三阶卡尔曼仿真及验证\Matlab仿真\KalmanHight.m
     目录           0  2019-05-10 14:49  三阶卡尔曼仿真及验证\STM32验证\
     目录           0  2019-05-10 14:48  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\
     目录           0  2019-05-10 14:48  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\CORE\
     文件      109142  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\CORE\core_cm4.h
     文件       22735  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\CORE\core_cm4_simd.h
     文件       17146  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\CORE\core_cmFunc.h
     文件       20513  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\CORE\core_cmInstr.h
     文件       29605  2019-03-19 09:19  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\CORE\startup_stm32f40_41xxx.s
     目录           0  2019-05-10 14:48  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\
     目录           0  2019-05-10 14:48  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\
     文件        6924  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\misc.h
     文件       32880  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_adc.h
     文件       27318  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_can.h
     文件        2416  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_crc.h
     文件       14481  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_cryp.h
     文件       14946  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_dac.h
     文件        4296  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_dbgmcu.h
     文件       12977  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_dcmi.h
     文件       28882  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_dma.h
     文件       19692  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_dma2d.h
     文件        8012  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_exti.h
     文件       24467  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_flash.h
     文件        3275  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_flash_ramfunc.h
     文件       44924  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_fmc.h
     文件       27181  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_fsmc.h
     文件       23548  2016-04-15 21:58  三阶卡尔曼仿真及验证\STM32验证\MPU6050_1\FWLIB\inc\stm32f4xx_gpio.h
............此处省略134个文件信息