观测器（Observer）

组件源码仓库地址：https://github.com/ZJU-HelloWorld/HW-Components

适用观测器种类

龙伯格观测器 Luenberger，卡尔曼滤波器 Kalman，扩展卡尔曼滤波器 Ekf 和无迹卡尔曼滤波器 Ukf。

使用前准备

使用本组件前需要做以下准备：

• 在 config.cmake 文件中设置 use_hwcomponents_algorithms_observer 选项为 ON，开启该设备文件的编译，若为 OFF 则根驱动配置需求在 CMakeLists.txt 文件中重配置为 ON。

1. 设计说明

观测器在使用时应具有一下基本功能：

1. 通过输入控制量与观测量，然后进行滤波计算。
2. 可以获取最后状态估计量。
3. 使用者可以设定状态量的初值。
4. 根据使用者需要，需要提供重置观测器的接口。

2. 使用示例

在项目中引用头文件：

#include "observer.hpp"

namespace hw_observer = hello_world::observer;

实例化一个观测器并根据需要进行配置（以 Kalman 滤波器为例）：

float A[4] = {1, 0.001f, 0, 1};  // 系统矩阵
float B[2] = {0, 0.1f};          // 控制矩阵
float C[4] = {1, 0, 0, 1};       // 输出矩阵
hw_observer::Observer* observer_ptr = nullptr;

hw_observer::Kalman::Config config_kf;
config_kf.x_dim = 2;     // 状态量维度
config_kf.z_dim = 1;     // 观测量维度
config_kf.u_dim = 1;     // 控制量维度
config_kf.x0 = nullptr;  // 以零状态作为初始状态
float Q[] = {0.0000001f, 0.0f, 0.0f, 0.0000001f};
float R[] = {0.1f};
config_kf.Q = Q;  // 状态噪声协方差矩阵
config_kf.R = R;  // 观测噪声协方差矩阵
config_kf.F = A;
config_kf.H = C;
config_kf.B = B;
config_kf.P = nullptr;  // 以单位对角阵作为状态估计协方差矩阵

observer_ptr = new hw_observer::Kalman(config_kf);

以固定频率调用 calc 方法实现观测器计算。

float u;
float z;

/* 控制量与观测量赋值 */

observer_ptr->calc(&u, &z);

然后可通过 getX 方法获取最优状态估计量：

float x_hat[2];

observer_ptr->getX(x_hat);

可通过 setX0 方法设置状态量初值：

float x0[2] = {1, 0};

observer_ptr->setX0(x0);

可通过 reset 方法进行观测器重置：

observer_ptr->reset();

3. 特殊情况说明

由于观测器往往用于处理连续数据（不与离散数据对应），对于角度这类周期性数据，跨周期产生的跳变将严重影响滤波器的工作效果，因此需要使用 PeriodData2ContData（对于角度还可以使用 PeriodAngle2ContAngleRad 或 PeriodAngle2ContAngleDeg）类，将周期数据转换为连续数据后，再使用观测器进行计算。

附录

版本说明

版本号	发布日期	说明	贡献者
	2024.07.27	发布观测器使用手册	蔡坤镇