17、双轴旋转姿态估计的卡尔曼滤波器实时实现

双轴旋转姿态估计的卡尔曼滤波器实时实现

1. 引言

在姿态估计领域，卡尔曼滤波器是一种常用的算法。本文将详细介绍一个用于双轴旋转姿态估计的卡尔曼滤波器的实时实现程序，该程序主要涉及两个文件： RTATT2IMU.c 和 RTATT2IMU.h 。

2. 程序概述

2.1 主要功能

该程序实现了离散卡尔曼滤波器算法，用于估计 3 - Space Sensor LX IMU 的姿态。它使用四元数方向作为状态向量，并根据加速度计测量得到的测量姿态四元数进行计算。程序运行时间由用户在 IVALS.txt 文件中定义。

2.2 假设条件

• 旋转仅在两个轴上进行。
• 矩阵 Q 、 H 、 R 和初始 PA 均为“缩放单位矩阵”。

3. RTATT2IMU.c 文件详解

3.1 头文件和自定义函数声明

#include “RTATT2IMU.h”
#include <conio.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

// Declarations for CUSTOM FUNCTIONS to