资源简介
PX4 EKF MATLAB代码参考,建立以角速度,角加速度,重力加速度,磁通量共12阶的数据为状态的状态方程。以角速度、加速度、磁通量建立量测方程进行EKF滤波
![](http://www.nz998.com/pic/72171.jpg)
代码片段和文件信息
function [ x_aposterioriP_aposterioriRotMatrixrollpitchyaw] = AttltitudeEKF( dtz qrx_aposteriori_kP_aposteriori_k)
%AttltitudeEKF 状态估计的拓展卡尔曼滤波方法
%输入:
% updateVect:指示哪些数据进行了更新
% dt:更新周期
% z:测量值
% q:系统噪声,r:测量噪声
% H_k:测量矩阵
% x_aposteriori_k:上一时刻的状态估计
% P_aposteriori_k:上一时刻估计协方差
%输出:
% x_aposteriori:当前时刻的状态估计
% P_aposteriori:当前时刻的估计协方差
% RotMatrix:旋转矩阵
% roll、pitch、yaw:欧拉角
% ? O=[ 0wz -wy;
% ? ? ? -wz 0 wx;
% ? ? ? wy -wx 0];
O=[0x_aposteriori_k(3)-x_aposteriori_k(2);
-x_aposteriori_k(3)0x_aposteriori_k(1);
x_aposteriori_k(2)-x_aposteriori_k(1)0];
bCn = eye(33) +O*dt;%旋转矩阵
%更新先验状态矩阵
x_apriori(1:3) = x_aposteriori_k(1:3) + x_aposteriori_k(4:6)*dt;%角速度
x_apriori(4:6) = x_aposteriori_k(4:6);%角加速度
x_apriori(7:9) = bCn*x_aposteriori_k(7:9);%加速度
x_apriori(10:12) = bCn*x_aposteriori_k(10:12);%磁场
%更新状态转移矩阵
% r_a=[ 0 -az ay; r_m=[ 0 -mz my;
% az 0 -ax; mz 0 -mx;
% -ay ax 0]; -my mx 0];
r_a = [0-x_aposteriori_k(9)x_aposteriori_k(8);
x_aposteriori_k(9)0-x_aposteriori_k(7);
-x_aposteriori_k(8)x_aposteriori_k(7)0];
r_m = [0 -x_aposteriori_k(12)x_aposteriori_k(11);
x_aposteriori_k(12)0-x_aposteriori_k(10);
-x_aposteriori_k(11)x_aposteriori_k(10)0];
A_lin = [eye(33)eye(33)*dtzeros(33)zeros(33);
zeros(33)eye(33)zeros(33)zeros(33);
r_a*dtzeros(33)(eye(33)+O*dt)zeros(33);
r_m*dtzeros(33)zeros(33)(eye(33)+O*dt)];
%预测误差协方差矩阵
Q=[eye(33)*q(1)zeros(33)zeros(33)zeros(33);
zeros(33)eye(33)*q(2)zeros(33)zeros(33);
zeros(33)zeros(33)eye(33)*q(3)zeros(33);
zeros(33)zeros(33)zeros(33)eye(33)*q(4)];
P_apriori = A_lin*P_aposteriori_k*A_lin‘+A_lin*Q*A_lin‘;
%卡尔曼增益
R=[eye(33)*r(1)zeros(33)zeros(33);
zeros(33)eye(33)*r(2)zeros(33);
zeros(33)zeros(33)eye(33)*r(3)];
H_k=[eye(33)zeros(33)zeros(33)zeros(33);
zeros(33)zeros(33)eye(33)zeros(33);
zeros(33)zeros(33)zeros(33)eye(33)];
K_k=(P_apriori*H_k‘)/(H_k*P_apriori*H_k‘+R);
%状态估计矩阵
x_aposteriori=x_apriori‘+K_k*(z - H_k*x_apriori‘);
%估计误差协方差
% P_aposteriori=(eye(1212)-K_k*H_k)*P_apriori;%计算方式简单但是容易失去正定性,PX4中使用这种方式
P_aposteriori=(eye(1212)-K_k*H_k)*P_apriori*(eye(1212)-K_k*H_k)‘+K_k*R*K_k‘;
R=eye(33)*r(1);
H_k=[eye(33)zeros(33)zeros(33)zeros(33)];
K_k=(P_apriori*H_k‘)/(H_k*P_apriori*H_k‘+R);
x_aposteriori=x_apriori‘+K_k*(z(1:3) - H_k*x_apriori‘);
P_aposteriori=(eye(1212)-K_k*H_k)*P_apriori*(eye(1212)-K_k*H_k)‘+K_k*R*K_k‘;
R=[eye(33)*r(1)zeros(33);
zeros(33)eye(33)*r(2)];
H_k=[eye(33)zeros(33)zeros(33)zeros(33);
zeros(33)zeros(33)eye(33)zeros(33)];
K_k=(P_apriori*H_k‘)/(H_k*P_apriori*H_k‘+R);
x_aposteriori=x_apriori‘+K_k*(z(1:
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 4031 2018-10-29 15:31 PX4EKF MATLAB代码参考\AttltitudeEKF.m
目录 0 2019-01-09 15:28 PX4EKF MATLAB代码参考
----------- --------- ---------- ----- ----
4031 2
相关资源
- Matlab路面裂缝识别69319
- 高灵敏度GPS接收机MATLAB仿真,附捕获
- 基于MATLAB的质点弹道计算与外弹道优
- 阵列天线的matlab仿真
- MATLAB 经典程序源代码大全
- MATLAB小波软阈值去噪代码33473
- 天线阵的波束形成在MATLAB仿真程序及
- 非线性SVM算法-matlab实现
- 《MATLAB 智能算法超级学习手册》-程序
- 组合导航matlab程序
- 读取txt文件内容matlab代码实现
- Matlab实现基于相关的模板匹配程序
- matlab优化工具箱讲解
- 基于MATLAB的快速傅里叶变换
- 光纤传输中的分布傅立叶算法matlab实
- 基于matlab的图像处理源程序
- matlab 椭圆拟合程序
- 算术编码解码matlab源代码
- optical_flow 光流法 matlab 实现程序
- 引导图像滤波器 Matlab实现
- 分形几何中一些经典图形的Matlab画法
- OFDM系统MATLAB仿真代码
- SVM工具箱(matlab中运行)
- 图像小波变换MatLab源代码
- LU分解的MATLAB实现
- 冈萨雷斯数字图像处理matlab版(第三
- 替代数据法的matlab程序
- 用matlab实现的多站定位系统性能仿真
- 通过不同方法进行粗糙集属性约简m
- k近邻算法matlab实现
评论
共有 条评论