链接检索
- BP-TWR-50使用
[DWM1000 定位操作流程–蓝点无限]
- 上位机软件
- 上位机工作流程
- 测距原理
[DWM1000 多个基站定位讨论 –蓝点无限]
[DWM1000 测距原理简单分析 之 SS-TWR代码分析2 – 蓝点无限]
[DWM1000 测距原理简单分析 之 SS-TWR代码分析1 – 蓝点无限]
- 如何使用三边定位代码
- 如何使用卡尔曼滤波代码
BP-TWR-50使用
[DWM1000 定位操作流程–蓝点无限]
外观
- 基站和标签硬件一样，其中标签有液晶插座，可插液晶，显示与基站之间的距离。
接口
使用场景
测距场景
- 基站0与标签通过收、发信息之间的传播时间计算距离。
跟随小车场景
- 基站0与基站1分别放到小车头部的两端，由标签牵引小车前进的方向。可设置小车的启动距离，当大于该距
离时，小车启动；小于该距离，则停止。行动过程中，采用余弦定理，计算基站0与标签的内角度，根据该
角度和基站0与标签的距离控制小车移动。
二维定位场景
- 基站0、基站1、基站2分别放置到同一平面，且都与标签通信 。
三维定位场景
- 基站0、基站1、基站2分别放置到同一平面，基站3放置Z轴，所有基站与标签通信。
多标签定位场景
- 多个标签同时与基站通信，二维、三维都支持多标签定位 。
上电检查
仅标签上电
仅基站上电
基站、标签都上电
- 基站和标签指示灯一样。
- 标签显示与基站的距离，如图an0为与基站0的距离，若有多个，则依次显示an1、an2。
放置模块
测距场景
- 模块之间距离大于20厘米，太近会影响测距精度，且模块之间不要有障碍物，影响模块之间通信。
跟踪小车场景
- 基站放置到小车的两端，两个基站的距离大于30厘米，前方由标签牵引。
定位场景
- 将烧录好程序的基站放置到指定位置，若三个基站，则放置为三角形，建议等边三角形。四个基站，则放置
为矩形，建议正方形。 - 对于3基站1标签定位，只能实现2D定位，即所有模块需要在一个平面上，推荐使用等高的移动电源供电，所
有模块都垂直放置到同一水平面。模块之间不要放置障碍物，障碍物对于定位误差影响很大。 - 基站0和基站1在x轴上，基站2在y轴上。举例如下图所示，基站0放置的坐标是(0,0)，基站1放置在x轴上，基
站2放置在 (x,y) ，可以在(5,8)处，5是中点，8是自定义值。 - 由于通信定位计算中有一定的误差，标签活动范围需要在上图的实心黄色部分。
标签基站的选择
//#define TAG 若放开注释，则当前设置为标签
#define TAG_ID 0x0F//设置当前标签ID
#define MASTER_TAG 0x0F
#define MAX_SLAVE_TAG 0x02//从标签最多有0，1，2三个
#define SLAVE_TAG_START_INDEX 0x01//从标签ID从0x01开始
#define ANTHOR//当前设置为基站
#define ANCHOR_MAX_NUM 3 // 0 1 2
#define ANCHOR_IND 2 //设置当前基站ID
标签
- TAG 为标签开关，标签分为MASTER TAG和SLAVE TAG。
- MASTER TAG负责调度；SLAVE TAG分配的测距任务。
- ANTHOR 为基站开关。
- TAG与ANTHOR只能选择其中一个，使用时需删除前面的“//”。
- TAG_ID 定义SLAVE TAG。
- 一个标签时，该TAG值需与MASTER_TAG的值一样。
- 多个标签时，除了第一个标签与MASTER_TAG的值一样，其它标签 时，该TAG值可自定义，该值不能与其他值重复。
- 如果只有一个主标签，那么TAG_ID设置为0x0F，编译；如果还有其他从标签，那么TAG_ID依次设置为0x01，0x02……，分别编译。
- MASTER TAG
- 定义MASTER TAG，系统中必须有MASTER TAG，MASTER_TAG值无需修改。
- MAX_SLAVE_TAG
- 定义系统中的SLAVE TAG个数。
- SALVE_TAG_START_INDEX 无需修改该值。
基站
- ANTHOR_MAX_NUM 定义基站总数。
- ANTHOR_IND 基站ID，从0开始，若基站共有3个，分别修改分别编译基站0 、1 、2 。
连接
- 如下图所示，串口线黑色连接标签GND，串口线白色连接标签TX，其他不用连接，串口另一端连接到电
脑上。
| 串口线 | 模块 |
|---|---|
| 白色(RXD) | TX |
| 黑色(GND) | GND |
| 红色(+5V) | 不连接 |
| 绿色（TXD） | 不连接 |
调试
- 连接到串口助手，串口软件界面显示如下，为上电模块初始化信息。
- 如果没有输出信息，请按复位键。如果还是没有输出信息，请检查模块是否上电，以及模块串口连线、
串口软件的端口软件开关是否正常。
- 重复上述方法分别连接基站，查看基站串口信息。
- 在标签未上电时，基站的串口显示数据和标签相同。
- 标签上电时，除了基站0的其他基站串口数据与标签相同，基站0仅在上电瞬间或复位瞬间时显示初始化
信息，后续则为距离信息。界面选择HEX，可查看串口信息。
-
6D 72 02 0F 21 0C F6 00 AB 00 30 00 F6 00 0A 0D
-
‘m’ 和‘r’为自定义数据头。
-
0x02为版本号。
-
TAG_ID为标签ID，多标签时可通过这个ID区别。
-
Frame_1、Frame_2 为帧序列号。
-
Dis_x_L、Dis_x_H为基站与标签的距离，其中Dis_x_L表示低8位，Dis_x_H表示高8位。
-
3个基站时，最后的两个为基站0到标签的距离，与前面的Dis_0_L、Dis_0_H相同。
-
4个基站时，最后的两个为第四个基站与标签的距离。
-
\r’和‘\n’为自定义数据结束，换行。
此处是用的是三个基站
0x6D为’m‘ 0x72为’r‘ 自定义数据头
0x02为版本号
0x0F为标签ID
0x21为Frame_1，0x0C为Frame_2
Dis_0_L为0xF6 Dis_0_H为0x00
Dix_X_L与Dis_0_L相同 Dis_X_H与Dis_0_H相同
Dis_1_L为0xAB Dis_1_H 为0x00
Dis_2_L为0x30 Dis_2_H 为0x00
0x0A为‘/r’（换行） 0x0D为’/n‘（回车）
上位机软件
- 基站0连接串口。
- 标签上电时，除了基站0的其他基站串口数据与标签相同，基站0仅在上电瞬间或复位瞬间时显示初始化信息，后续则为距离信息。界面选择HEX，可查看串口信息。
- 按照实际坐标与距离进行填写，单击“Connect”。
- 标签信息会自动显示在界面上，建议标签在基站包围的范围内运行。
上位机工作流程
-
先说下上位机大概的流程：
- 上位机通过串口接收数据，收到数据后进行数据包解析，解析得到三个距离，配合上位机已经设定的三个基站的坐标，通过三边定位算法解算标签的坐标。如果坐标解算成功，把对应的点在界面上显示。
-
主要涉及到的文件：
- 1 SerialConnection.cpp 这个文件负责串口初始化相关内容
- 2 RTLSClient.cpp
-
着重分析下数据处理流程，这部分功能是RTLSClient.cpp中实现的
- 1 函数void RTLSClient::newData()
- 当收到串口信息会调用这个函数，在这个函数中会判断是否接受到一个完整的数据包，这个数据包必须是固件和上位机约定的格式，例如头必须是mr。否则认为数据包无效。
- 2 函数 void RTLSClient:: ProcessData(void)
- 这个函数是当接收到有效数据包后，对数据包里的数据进行解析，解析主要内容有3个距离信息(如果是4基站定位有4个距离信息)，标签的ID以及其它信息。
- 3processTagRangeReport(0, TAG_ID, range[0]*10, lnum, seq);
- 这个函数是处理TAG和某个基站的距离信息，可以适当做校正，类似于滤波，把距离信息存放到某个大的结构体里。
- 4 trilaterateTag(TAG_ID, seq);
- 这个是解算标签坐标的入口函数，在这个函数会判断是有几个基站距离，然后调用calculateTagLocation 完成解算。
- 5 int RTLSClient::calculateTagLocation(vec3d *report, int count, int *ranges)
- 其实这个函数中进一步提前去基站设定的坐标，匹配距离信息送到GetLocation，GetLocation 是一个纯数学的东西了。
- 6emit tagPos(tid, report.x, report.y, report.z);
- 当calculateTagLocation 执行成功，标签的坐标解算出来，会emit这个信号，槽收到这个信号，将标签的坐标进行绘制。
- 1 函数void RTLSClient::newData()
-
总体代码流程就是这样。 具体细节需要仔细研读代码，并且对QT 的信号和槽有较深入的理解才能更好的追逐代码。
例程源码解析
【新提醒】咨询bp50的代码问题 - 问答社区 51uwb.cn
main.c
#ifdef ANTHOR （卡尔曼滤波距离信息）
#ifdef ANTHOR
Anchor_Array_Init(); // 初始化锚点数组
/* Loop forever initiating ranging exchanges. */
OLED_ShowString(0,0," 51UWB Node"); // 在OLED显示屏上显示"51UWB Node"
sprintf(dist_str, " ANTHOR:%02X", ANCHOR_IND); // 格式化并显示锚点指示
OLED_ShowString(0,2,dist_str); // 在OLED显示屏上显示锚点指示
OLED_ShowString(0,6," www.51uwb.cn"); // 显示网址
while (1) // 无限循环
{
/* Clear reception timeout to start next ranging process. */
dwt_setrxtimeout(0); // 清除接收超时，准备下一个测距过程
/* Activate reception immediately. */
dwt_rxenable(0); // 立即激活接收
/* Poll for reception of a frame or error/timeout. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ }; // 轮询接收帧或错误/超时
if (status_reg & SYS_STATUS_RXFCG) // 如果接收到帧
{
/* Clear good RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); // 清除DW1000状态寄存器中的良好接收帧事件
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; // 读取接收到的帧长度
if (frame_len <= RX_BUFFER_LEN) // 如果帧长度小于或等于缓冲区长度
{
dwt_readrxdata(rx_buffer, frame_len, 0); // 读取帧数据到缓冲区
}
/* Check that the frame is a poll sent by "DS TWR initiator" example. */
if(rx_buffer[ALL_MSG_SN_IDX] % ANCHOR_MAX_NUM != ANCHOR_IND) // 检查序列号是否符合当前锚点索引
continue; // 如果不符合，继续下一次循环
anthor_index = rx_buffer[ALL_MSG_SN_IDX] % ANCHOR_MAX_NUM; // 获取锚点索引
tag_index = rx_buffer[ALL_MSG_TAG_IDX]; // 获取标签索引
rx_buffer[ALL_MSG_SN_IDX] = 0; // 清除序列号字段
rx_buffer[ALL_MSG_TAG_IDX] = 0; // 清除标签索引字段
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) // 检查是否为轮询消息
{
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64(); // 获取轮询接收时间戳
/* Set expected delay and timeout for final message reception. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS); // 设置最终消息接收的预期延迟和超时
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS); // 设置最终消息接收的超时时间
/* Write and send the response message. */
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb; // 设置响应消息的序列号
tx_resp_msg[ALL_MSG_TAG_IDX] = tag_index; // 设置响应消息的标签索引
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0); // 写入响应消息数据
dwt_writetxfctrl(sizeof(tx_resp_msg), 0); // 设置响应消息的帧控制
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); // 开始传输，并期望响应
// 等待接收帧或超时/错误
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
// 如果成功接收到最终消息
if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); // 清除DW1000状态寄存器中的良好接收帧事件
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; // 读取接收到的帧长度
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0); // 读取帧数据到缓冲区
}
// 检查TAG_ID是否匹配
if(tag_index != rx_buffer[ALL_MSG_TAG_IDX])
continue; // 如果不匹配，继续下一次循环
rx_buffer[ALL_MSG_TAG_IDX] = 0; // 清除标签索引字段
/*As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0; // 清除序列号字段
// 检查是否为最终消息
if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
// 声明变量用于存储轮询发送、响应接收和最终发送的时间戳
uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
// 用于存储32位版本的时间戳
double Ra, Rb, Da, Db;
// 声明变量用于存储时间差计算结果
int64 tof_dtu;
// 声明变量用于存储飞行时间（以设备时间单位表示）
/* Retrieve response transmission and final reception timestamps. */
resp_tx_ts = get_tx_timestamp_u64();
// 获取响应发送时间戳
final_rx_ts = get_rx_timestamp_u64();
// 获取最终接收时间戳
/* Get timestamps embedded in the final message. */
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts); // 从最终消息中提取轮询发送时间戳
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts); // 提取响应接收时间戳
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts); // 提取最终发送时间戳
/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
poll_rx_ts_32 = (uint32)poll_rx_ts; // 转换为32位变量
resp_tx_ts_32 = (uint32)resp_tx_ts; // 转换为32位变量
final_rx_ts_32 = (uint32)final_rx_ts; // 转换为32位变量
Ra = (double)(resp_rx_ts - poll_tx_ts); // 计算时间差Ra
Rb = (double)(final_rx_ts_32 - resp_tx_ts_32); // 计算时间差Rb
Da = (double)(final_tx_ts - resp_rx_ts); // 计算时间差Da
Db = (double)(resp_tx_ts_32 - poll_rx_ts_32); // 计算时间差Db
tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db)); // 计算飞行时间
tof = tof_dtu * DWT_TIME_UNITS; // 将飞行时间转换为标准时间单位
distance = tof * SPEED_OF_LIGHT; // 计算距离
distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//距离减去矫正系数
//将计算结果发送给TAG
int temp = (int)(distance*100); // 将距离转换为整数形式
distance_msg[10] = temp/100; // 计算距离的整数部分
distance_msg[11] = temp%100; // 计算距离的小数部分（两位）
distance_msg[12] = anthor_index; // 加入其它索引信息
distance_msg[ALL_MSG_SN_IDX] = frame_seq_nb; // 设置帧序列号
distance_msg[ALL_MSG_TAG_IDX] = tag_index; // 设置标签索引
dwt_writetxdata(sizeof(distance_msg), distance_msg, 0); // 写入待发送的距离消息
dwt_writetxfctrl(sizeof(distance_msg), 0); // 设置发送控制信息
/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
dwt_starttx(DWT_START_TX_IMMEDIATE ); // 开始立即发送距离消息
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); // 清除DW1000状态寄存器中的接收错误事件
}
}
// 其他处理逻辑省略，因为注释超出了长度限制
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); // 清除DW1000状态寄存器中的接收错误事件
}
}
#endif
这段代码看起来是用于一个基于DW1000 UWB（超宽带）芯片的定位系统。代码主要实现的功能是处理从其他节点（可能是标签或锚点）接收到的UWB信号，并计算距离。下面是对代码的逐行分析：
-
预处理指令:
#ifdef ANTHOR: 这意味着以下代码仅在定义了ANTHOR这个预处理变量时才会编译。
-
初始化和显示设置:
Anchor_Array_Init();: 初始化锚点数组。OLED_ShowString(0,0,"51UWB Node");: 在OLED屏幕上显示文本"51UWB Node"。sprintf(dist_str, "ANTHOR:%02X", ANCHOR_IND);: 格式化字符串，显示当前锚点的索引。OLED_ShowString(0,2,dist_str);: 显示锚点索引。OLED_ShowString(0,6,"www.51uwb.cn");: 在OLED上显示网址。
-
主循环:
while (1): 这是一个无限循环，用于不断接收和处理数据。
-
接收数据设置:
dwt_setrxtimeout(0);: 设置DW1000的接收超时时间。dwt_rxenable(0);: 启用接收。
-
等待接收数据:
- 通过检查状态寄存器来等待接收到的数据或者错误/超时。
-
处理接收到的数据:
- 如果接收到数据（
status_reg & SYS_STATUS_RXFCG），会读取数据并根据内容进行不同的处理。 - 代码主要处理两种类型的消息：范围测量请求（poll message）和角度测量消息（angle message）。
- 在处理范围测量请求时，它计算了来回信号的时间差（time of flight），进而计算出距离。这个距离值被进一步处理并发送给请求节点。
- 在处理角度测量消息时，它要么发送位置信息，要么控制某个动作（如跟随车辆）。
这段代码是使用超宽带(UWB)信号进行时间飞行(ToF)距离测量的一部分，特别是在使用Decawave设备的上下文中，根据使用的函数和术语可以看出。这是一个用C语言编写的程序片段，用于使用精确的时间测量计算发射器和接收器之间的距离。下面是逐步分解：
1. 变量声明：
- uint32, int64, double: 这些是表示时间戳、时间间隔和其他计算的变量的数据类型。
- poll_tx_ts, resp_rx_ts, final_tx_ts: 通信序列中各个事件（询问、响应和最终消息的发送和接收）的时间戳变量。
- tof_dtu: 设备时间单位（DTUs）中的飞行时间变量。
2. 检索时间戳：
- resp_tx_ts, final_rx_ts: 这些函数分别检索响应和最终消息的发送和接收时间戳，可能来自UWB硬件。
- final_msg_get_ts: 一个函数，用于从接收到的最终消息中提取时间戳。像
FINAL_MSG_POLL_TX_TS_IDX这样的索引指的是消息中这些时间戳位于的特定位置。
3. 飞行时间计算：
- 代码基于四个时间戳使用双向双程算法计算飞行时间。这涉及到计算发送和接收消息之间的往返时间并解算飞行时间。
- Ra, Rb, Da, Db: 这些是表示发送和接收消息之间的各种时间差的中间变量。
- 公式
tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));计算设备时间单位(DTUs)中的飞行时间。
4. 距离计算：
- tof: 通过将
tof_dtu乘以DWT_TIME_UNITS（定义一设备时间单位的时间持续的常量）将飞行时间转换为秒（或其他时间单位）。 - distance: 然后使用公式
distance = tof * SPEED_OF_LIGHT;计算距离。这将飞行时间转换为物理距离。 - 行
distance = distance - dwt_getrangebias(...);通过减去偏差值调整计算的距离，以考虑设备特定或环境因素。
5. 准备距离消息：
-
代码接着准备一个消息（
distance_msg），似乎包括了计算出的距离和一些其他信息，如索引和序列号。这个消息很可能会发回给原始设备或另一个系统组件。 -
这段代码是处理测量得到的距离值，将其转换成整数和小数部分，以便于进一步的信息传递或显示。这里的数学原理是基于整数和余数运算。具体来看：
代码功能和流程：
-
距离转换：
int temp = (int)(distance*100);- 这行代码首先将距离（以米为单位假定）乘以100，目的是将米转换为厘米，并保留两位小数的精度（因为距离可能是一个浮点数）。然后，通过转换为整数（int），去掉了小数部分后的所有数字。这样，
temp现在是一个整数，表示距离的厘米部分，包括原始距离的整数和小数部分。
- 这行代码首先将距离（以米为单位假定）乘以100，目的是将米转换为厘米，并保留两位小数的精度（因为距离可能是一个浮点数）。然后，通过转换为整数（int），去掉了小数部分后的所有数字。这样，
-
整数部分计算：
distance_msg[10] = temp/100;- 这行代码通过整除100，从
temp中提取出原始距离的整数部分。因为temp是以厘米为单位的距离，所以除以100后得到的是以米为单位的整数部分。这个值被存储在distance_msg数组的第10位，可能用于后续的显示或通信。
- 这行代码通过整除100，从
-
小数部分计算：
distance_msg[11] = temp%100;- 这行代码通过取余数的方式从
temp中提取出原始距离的小数部分（两位）。%100的操作实际上是取了temp除以100后的余数，即厘米部分的最后两位数字，它们代表了原始距离的小数部分。这个值被存储在distance_msg数组的第11位，同样可能用于后续的显示或通信。
- 这行代码通过取余数的方式从
数学原理：
-
整数转换与乘法： 将浮点数乘以100并转换为整数是为了捕捉原始浮点数的小数部分。例如，如果原始距离是
3.276米，乘以100后为327.6，转换为整数后为327。 -
整除与取余： 整除（
/）和取余（%）是整数运算的基本操作，用于提取整数中的特定位数。整除用于提取较高位数，而取余用于提取较低位数。
通过这种方式，代码能够将浮点数表示的距离分解为整数和小数部分，并将其以整数形式存储，便于消息传递或其他处理。这种处理在涉及数字显示或者需要将数字以特定格式传递时非常常见，特别是在嵌入式系统或硬件通信中，这些系统可能不直接支持浮点数的处理或显示。
-
-
距离被分解成主要和次要部分（比如整数和小数部分）并填充到消息中。
6. 距离消息的传输：
- 最后，消息被加载到设备的传输缓冲区并发送。
dwt_starttx(DWT_START_TX_IMMEDIATE);调用触发了准备好的消息的即时传输。
注释：
- 错误处理： 这个片段中没有明确的错误处理，这在现实世界的应用程序中是非常重要的。
- 常量和函数： 代码使用特定的常量（如
DWT_TIME_UNITS,SPEED_OF_LIGHT）和函数（如dwt_writetxdata,dwt_writetxfctrl等），这些很可能是Decawave或类似供应商提供的特定UWB库的一部分。 - 注释代码： 如
//将计算结果发送给TAG等行是中文注释，指示减去校正系数和发送计算结果等操作。
这段代码片段特定于特定的硬件和应用上下文，很可能是UWB测距，其中需要精确的距离测量，如室内定位、资产跟踪或接触追踪应用。
- 如果接收到数据（
-
错误处理:
- 如果接收过程中出现错误，会清除错误标志并继续等待接收新的数据。
-
结束预处理指令:
#endif: 这标记了在#ifdef ANTHOR开始的代码块的结束。
总体来说，这段代码是用于UWB定位系统中的一个锚点，用于接收来自标签的信号，计算距离，并根据需要发送回应或进行其他操作。代码涉及硬件操作、数据接收、时间戳处理和通信，这表明它是嵌入式系统或物联网应用的一部分。
#ifdef TAG
#ifdef TAG
/* 如果定义了TAG，则执行以下代码块 */
/* 设置预期响应的延迟和超时时间。参见下面的注释4和5。 */
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); // 设置发送后接收开始的延迟时间
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); // 设置接收超时时间
/* 根据TAG_ID值显示不同的信息 */
if(TAG_ID == MASTER_TAG)
{
/* 如果是主节点，则在OLED上显示相应信息 */
OLED_ShowString(0,0," 51UWB 节点"); // 显示"51UWB 节点"
OLED_ShowString(0,6," www.51uwb.cn"); // 显示网址"www.51uwb.cn"
OLED_ShowString(0,2," 主节点 "); // 显示"主节点"
}
else
{
/* 如果是从节点，则在OLED上显示相应信息 */
OLED_ShowString(0,0," 51UWB 节点"); // 显示"51UWB 节点"
OLED_ShowString(0,6," www.51uwb.cn"); // 显示网址"www.51uwb.cn"
OLED_ShowString(0,0," 从节点 "); // 显示"从节点"
}
/* 根据TAG_ID管理信号量 */
if(TAG_ID == MASTER_TAG)
{
/* 主节点启用信号量并进行初始化 */
Semaphore_Enable = 1 ; // 启用信号量
Semaphore_Init(); // 初始化信号量
Waiting_TAG_Release_Semaphore = 0; // 初始化等待释放信号量的标记
}
else
{
/* 从节点禁用信号量 */
Semaphore_Enable = 0 ; // 禁用信号量
}
/* 主节点循环处理逻辑 */
while(1)
{
/* 检查信号量是否启用 */
if(Semaphore_Enable == 1)
{
/* 重置GPIO引脚 */
GPIO_ResetBits(GPIOA,GPIO_Pin_1);
GPIO_ResetBits(GPIOA,GPIO_Pin_2);
/* 发送测距请求，并计算距离 */
Tag_Measure_Dis();// 测量节点与所有基站之间的距离
Semaphore_Enable = 0 ; // 测量后关闭信号量
/* 如果是从节点，则发送释放信号量的消息给主节点 */
if(TAG_ID != MASTER_TAG)
{
/* 组装并发送释放信号量的消息 */
Semaphore_Release[ALL_MSG_SN_IDX] = frame_seq_nb;
Semaphore_Release[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(Semaphore_Release), Semaphore_Release, 0); // 写入释放信号量消息
dwt_writetxfctrl(sizeof(Semaphore_Release), 0); // 设置发送控制
/* 开始发送释放信号量消息，并等待发送完成 */
dwt_starttx(DWT_START_TX_IMMEDIATE );
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
GPIO_SetBits(GPIOA,GPIO_Pin_2); // 通过GPIO指示发送完成
}
}
/* 主节点处理各个从节点的统计和信号量管理 */
if(TAG_ID == MASTER_TAG)
{
/* 统计并管理各个节点的信号量 */
if(Sum_Tag_Semaphore_request() == 0)
{
for(tag_index = SLAVE_TAG_START_INDEX; tag_index <MAX_SLAVE_TAG; tag_index++)
{
// ...（省略了部分中间的代码，此处主要包括统计和管理信号量的逻辑）
}
/* 打印网络中的所有节点信息 */
for(tag_index = SLAVE_TAG_START_INDEX; tag_index <MAX_SLAVE_TAG; tag_index++)
{
// ...（省略了部分中间的代码，此处主要包括打印节点信息的逻辑）
}
}
/* 根据信号量请求选择一个从节点，并发送信号量消息 */
// ...（省略了部分中间的代码，此处包括信号量消息发送的逻辑）
}
else // 如果是从节点
{
// ...（省略了部分中间的代码，此处包括从节点接收和处理信号量的逻辑）
}
}
#endif
这段代码是一个针对基于DW1000芯片的UWB（超宽带）通信系统中标签（Tag）的行为的编程。代码实现了标签间的通信、距离测量、信号量处理以及与主标签（Master Tag）的交互。以下是对这段代码的逐行分析：
-
预处理指令:
#ifdef TAG: 这表示以下代码仅在定义了TAG这个预处理变量时编译。
-
设置响应的延迟和超时:
dwt_setrxaftertxdelay和dwt_setrxtimeout设置在发送轮询消息后的接收延迟和响应超时时间。
-
初始化显示:
- 根据
TAG_ID的值，OLED上显示不同的字符串来区分主标签和从标签（Slave Tag）。
- 根据
-
信号量（Semaphore）设置:
- 如果
TAG_ID是主标签，就初始化信号量并设置相关标志。
- 如果
-
主循环 (
while(1)):- 这是标签持续运行的主要部分。
-
测量距离:
- 如果启用了信号量，标签将测量与所有锚点之间的距离，并将信号量设置为关闭。
-
主标签的特殊行为:
- 对网络中的所有标签进行统计。
- 发送信号量释放消息给特定的从标签，并等待确认。
-
从标签的行为:
- 等待主标签的统计消息和信号量释放消息。
- 收到主标签的信号量释放消息后，确认并启用自身的信号量。
-
信号量管理:
- 如果主标签等待的信号量没有被释放（可能因为标签离开网络），它将强制取消信号量。
- 如果所有标签都被服务过，主标签将重新启用其信号量。
-
结束预处理指令:
#endif: 这标记了在#ifdef TAG开始的代码块的结束。
总体来说，这段代码在一个基于DW1000芯片的UWB系统中管理标签的行为，包括距离测量、信号量管理和与主标签的交互。这显示了嵌入式系统或物联网应用中对时间同步和资源管理的复杂性。
void Tag_Measure_Dis(void)
void Tag_Measure_Dis(void)
{
uint8 dest_anthor = 0, frame_len = 0; // 定义目标锚点索引和帧长度变量
float final_distance = 0; // 定义最终距离变量
for(dest_anthor = 0 ; dest_anthor < ANCHOR_MAX_NUM; dest_anthor++) // 遍历所有锚点
{
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); // 设置发送轮询消息后接收的延迟
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); // 设置接收超时时间
// 准备并发送轮询消息
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb; // 设置帧序列号
tx_poll_msg[ALL_MSG_TAG_IDX] = TAG_ID; // 设置TAG_ID，用于锚点识别
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0); // 向DW1000写入轮询消息数据
dwt_writetxfctrl(sizeof(tx_poll_msg), 0); // 设置传输帧控制
// 启动传输，等待响应
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
dwt_rxenable(0); // 手动启用接收模式
// 等待接收帧或超时/错误
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
GPIO_SetBits(GPIOA, GPIO_Pin_1); // 设置GPIO指示接收状态
if (status_reg & SYS_STATUS_RXFCG) // 如果成功接收到帧
{
// 清除DW1000状态寄存器中的接收事件和传输事件
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
// 读取接收到的帧到缓冲区
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
if(rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID) // 检查TAG_ID是否匹配
continue;
rx_buffer[ALL_MSG_TAG_IDX] = 0;
// 清除序列号字段以简化帧验证
rx_buffer[ALL_MSG_SN_IDX] = 0;
// 检查响应消息
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 final_tx_time;
// 获取轮询传输和响应接收的时间戳
poll_tx_ts = get_tx_timestamp_u64();
resp_rx_ts = get_rx_timestamp_u64();
// 计算最终消息的传输时间
final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(final_tx_time);
// 设置最终传输时间戳
final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;
// 在最终消息中写入所有时间戳
final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
// 写入并发送最终消息
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
tx_final_msg[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 0);
// 开始延迟传输，并期待响应
dwt_starttx(DWT_START_TX_DELAY
code_with_comments_part2 = """
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED); // 开始延迟传输，并期待响应
// 再次等待接收帧或超时/错误
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
if (status_reg & SYS_STATUS_RXFCG) // 如果成功接收到帧
{
// 清除DW1000状态寄存器中的接收和传输事件
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
// 读取接收到的帧到缓冲区
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
if(rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID) // 检查TAG_ID是否匹配
continue;
rx_buffer[ALL_MSG_TAG_IDX] = 0;
// 清除序列号字段以简化帧验证
rx_buffer[ALL_MSG_SN_IDX] = 0;
// 检查距离消息
if (memcmp(rx_buffer, distance_msg, ALL_MSG_COMMON_LEN) == 0)
{
// 累加处理距离数据
Anthordistance[rx_buffer[12]] += (rx_buffer[10] * 1000 + rx_buffer[11] * 10);
Anthordistance_count[rx_buffer[12]]++;
// 检查是否需要更新距离
int Anchor_Index = 0;
while(Anchor_Index < ANCHOR_MAX_NUM)
{
if(Anthordistance_count[Anchor_Index] >= ANCHOR_REFRESH_COUNT)
{
distance_mange(); // 处理距离数据
// 重置所有距离和计数
for(int i = 0; i < ANCHOR_MAX_NUM; i++)
{
Anthordistance_count[i] = 0;
Anthordistance[i] = 0;
}
break;
}
Anchor_Index++;
}
}
}
else
{
// 清除DW1000状态寄存器中的错误事件
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
}
else
{
// 清除DW1000状态寄存器中的错误事件
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
// 在测距交换之间执行延时
// deca_sleep(RNG_DELAY_MS);
frame_seq_nb++; // 帧序列号自增
}
}
这段代码是一个名为Tag_Measure_Dis的函数，它的目的是从一个标签（Tag）的角度出发，与多个锚点（Anthor, 可能是Anchor的拼写错误）进行通信，测量并管理与每个锚点的距离。以下是对该函数的详细步骤解释：
1. 初始化和配置：
- 通过一个循环，函数遍历所有锚点（
dest_anthor），对每个锚点执行测距操作。 - 设置了轮询消息后接收的延迟
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS)和接收超时dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS)。
2. 发送轮询消息并处理响应：
- 对于每个锚点，函数配置并发送轮询消息
tx_poll_msg，其中包含了当前的帧序列号和标签ID。 - 然后启动传输，并手动启用接收模式等待响应。
3. 接收响应并处理：
- 函数等待接收到响应或超时/错误发生。如果成功接收到响应（
SYS_STATUS_RXFCG），它会读取并处理该响应。 - 验证接收到的响应是否是针对当前标签的（通过比较
TAG_ID），并处理序列号。
4. 计算并发送最终消息：
- 如果响应是预期的，函数计算最终消息的发送时间，并准备最终消息，包括所有相关时间戳（轮询发送、响应接收、最终发送时间戳）。
- 发送最终消息并等待最终距离消息的响应。
5. 接收并处理最终距离消息：
- 如果收到最终距离消息（
distance_msg），函数将累加并处理每个锚点的距离数据。 - 检查是否达到了更新距离的条件（例如，收到足够数量的数据），如果是，则调用
distance_mange函数来管理和显示距离数据。
6. 距离数据管理：
- 通过
Anthordistance和Anthordistance_count数组来累计和跟踪每个锚点的距离数据。 - 一旦收集到足够的数据，函数将重置计数并更新显示。
7. 循环和帧序列号更新：
- 对每个锚点重复上述过程。
- 每完成一轮测距，帧序列号
frame_seq_nb自增，准备下一轮测距。
总结：
Tag_Measure_Dis函数是一个完整的测距流程实现，从发送轮询消息到接收响应，再到发送最终消息，并最终收集和管理距离数据。这种方式通常用于UWB系统中的标签来测量其与多个锚点之间的距离，进而可以用于后续的定位和跟踪应用。这个函数体现了典型的双向双边测距（DS-TWR）交互模式，并展示了如何处理通信过程中的数据和状态。
static void distance_mange(void)
static void distance_mange(void)
{
{
int Anchor_Index = 0; // 初始化锚点索引
while(Anchor_Index < ANCHOR_MAX_NUM) // 遍历所有锚点
{
if(Anthordistance_count[Anchor_Index] > 0 ) // 如果该锚点收到的距离数据次数大于0
{
// 对收到的距离数据进行过滤处理，可能用于平滑数据
Anthordistance[Anchor_Index] = filter((int)(Anthordistance[Anchor_Index] / Anthordistance_count[Anchor_Index]), Anchor_Index);
}
Anchor_Index++; // 移至下一个锚点
}
}
// 计算角度并将结果发送给锚点0
compute_angle_send_to_anthor0(Anthordistance[0], Anthordistance[1], Anthordistance[2]);
if(first_distance == 1) // 如果是第一次测量距离
{
first_distance = 0; // 重置标志
LCD_ClearLines(); // 清除LCD显示
}
// 显示每个锚点的距离信息
if(Anthordistance_count[0] > 0)
{
sprintf(dist_str, "an0:%3.2fm", (float)Anthordistance[0] / 1000);
OLED_ShowString(0, 2, dist_str); // 显示锚点0的距离
}
if(Anthordistance_count[1] > 0)
{
sprintf(dist_str, "an1:%3.2fm", (float)Anthordistance[1] / 1000);
OLED_ShowString(0, 4, dist_str); // 显示锚点1的距离
}
if(Anthordistance_count[2] > 0)
{
sprintf(dist_str, "an2:%3.2fm", (float)Anthordistance[2] / 1000);
OLED_ShowString(0, 6, dist_str); // 显示锚点2的距离
}
}
这个 distance_mange 函数主要负责处理和显示从不同锚点（Anchors）接收到的距离数据。它的工作流程如下：
-
平均距离计算:
- 函数开始于一个遍历所有锚点的循环。
- 对于每个锚点，如果有收到距离数据（即
Anthordistance_count[Anchor_Index]大于 0），它计算平均距离。 - 这通过将累计的距离值
Anthordistance[Anchor_Index]除以收到的数据次数Anthordistance_count[Anchor_Index]来实现。 - 计算的平均值通过
filter函数进一步处理，可能用于平滑或其他形式的数据处理。
-
角度计算和传输:
compute_angle_send_to_anthor0函数使用前三个锚点的平均距离值来计算某种角度，并将其发送到锚点 0。这可能用于确定标签相对于锚点的方向或位置。
-
判断是否为首次测量:
- 如果
first_distance标志被设置，这意味着这是第一次测量距离。在这种情况下，它会清除LCD显示，并重置first_distance标志。
- 如果
-
显示距离信息:
- 对于每个锚点，如果有有效的距离数据，函数将计算出的距离转换为米（由于原始数据以毫米为单位，因此除以1000），并使用
sprintf函数格式化为字符串。 - 然后使用
OLED_ShowString函数将这些距离信息显示在OLED屏幕上。
- 对于每个锚点，如果有有效的距离数据，函数将计算出的距离转换为米（由于原始数据以毫米为单位，因此除以1000），并使用
总的来说，distance_mange 函数负责处理收集到的距离数据，并据此更新显示设备上的信息。这是嵌入式定位系统中常见的数据处理和用户界面更新操作。通过这种方式，用户可以实时看到与每个锚点的距离信息，以及基于这些距离信息计算出的其他信息（如角度）。
这段代码基于超宽带（UWB）技术进行测距和定位，特别是使用了双边双向测距（DS TWR）方法。以下是其工作原理的详细解释：
测距原理（Double-sided Two-Way Ranging, DS TWR）
消息交换：
- 发起者发送“Poll”消息：首先，发起设备（或称为发起者）发送一个“Poll”消息给响应设备。
- 响应设备接收并回应：响应设备接收到“Poll”消息后，会在一定时间内回应一个“Response”消息。
- 发起者发送“Final”消息：发起设备再次发送一个“Final”消息给响应设备以完成交换过程。
时间戳记录：
- 在每次发送和接收消息时，设备都会记录时间戳。这样，对于一次完整的测距过程，会涉及到四个时间戳：Poll发送时间（(T_{SP})）、Poll接收时间（(T_{RP})）、Response发送时间（(T_{SR})）、Response接收时间（(T_{RR})）。
飞行时间计算：
- 通过这些时间戳，设备能够计算出信号的往返飞行时间（TOF）。实际的TOF是Response消息的往返时间减去Poll消息的往返时间，以消除设备处理延迟的影响。
- TOF（Time of Flight）的计算公式是由这些时间戳差异导出的，它涉及到对发送和接收时间戳的差值进行几何平均。
距离估算：
- 一旦得到TOF，就可以通过乘以光速（在空气中）来计算距离。由于信号以电磁波形式传播，其速度接近光速。
定位原理
多点测距：
- 在多点定位系统中，通常有多个锚点（Anchors）固定在已知位置，每个锚点与标签（Tag，即要定位的对象）进行双向测距。
收集距离信息：
- 标签会从多个锚点收集距离信息。例如，如果有三个锚点，标签会知道它与每个锚点的距离。
使用三边测量（Trilateration）定位：
- 一旦获得了与多个锚点的距离，就可以使用三边测量法来确定标签的位置。这种方法在二维空间需要至少三个锚点，在三维空间需要至少四个。
- 三边测量利用了圆（二维）或球（三维）的几何性质，每个锚点和标签间的距离定义了一个圆（或球）的半径。标签的位置是所有圆（或球）交点的解。
优化和误差处理：
- 实际应用中，受到信号干扰、非视距条件和环境因素的影响，接收的距离测量可能包含误差。
- 系统可能会采用滤波算法（如卡尔曼滤波）来平滑数据，并使用误差修正技术来提高定位准确性。
特定代码段内的应用
- 代码中涉及了设置UWB通信参数、发送和接收消息、计算时间戳和距离、以及可能的滤波处理等。
- 它还包含了处理不同类型的消息和错误，以确保测距和定位过程的准确性和稳定性。
通过这样的一系列复杂步骤，该系统能够准确测量距离，并在多点定位的场景下确定目标的位置。这种技术在需要精准位置信息的应用中非常有用，如仓库管理、智能导航、室内定位等。
在您提供的代码中，使用了超宽带（UWB）技术的双边双向测距（DS TWR）方法进行测距和定位。以下是如何在代码中体现这些原理的详细解释：
测距原理体现：
- 消息定义：
- 代码中定义了各种消息类型（例如
rx_poll_msg,tx_resp_msg,rx_final_msg等），这些是测距通信中的关键元素。这些消息被用于发起者和响应者之间的通信。
- 时间戳捕获：
- 使用函数如
get_tx_timestamp_u64()和get_rx_timestamp_u64()来获取发送和接收消息的时间戳。这些时间戳对于计算飞行时间（TOF）至关重要。
- 飞行时间（TOF）计算：
- 在响应者收到“Poll”消息并发送“Response”消息后，代码计算飞行时间。这涉及到从发送和接收的时间戳中提取信息，并使用它们来估算距离。
- 距离计算：
- 计算距离的逻辑在
distance_mange函数中体现，使用飞行时间和光速来计算标签和锚点之间的距离。定位原理体现：
- 多个锚点的处理：
- 代码中通过定义多个锚点（如
AnchorList数组）并计算每个锚点到标签的距离（Anthordistance数组），来准备进行多点定位。
- 三边测量（Trilateration）：
- 在函数
compute_angle_send_to_anthor0中，距离数据被用来进行定位计算。虽然具体的三边测量计算可能在设备的外部或后续处理中进行，代码通过收集和整理必要的距离信息来为三边测量定位做准备。
- 数据发送和接收：
- 定位信息或距离数据通过UWB消息发送给其他设备（可能是中心处理单元或其他锚点），以进行进一步处理或决策。
- 滤波和误差处理：
filter函数可能被用来平滑距离数据，这有助于减少测量噪声和提高定位准确性。代码中的特别说明：
- 代码使用了STM32和Decawave的DW1000 UWB芯片的特定库和API，这表明它高度依赖于硬件特性来实现测距和定位的功能。
- 代码中的各种配置参数（如脉冲频率、前导码长度等）对于UWB通信的性能和准确性至关重要。
- 通过改变硬件设置或调整代码逻辑，可以优化测距和定位的性能，例如改进时间同步精度或调整消息交换逻辑。
综上所述，该代码利用UWB技术的精确时间测量能力，通过发送和接收特定的消息，并准确记录时间戳来实现测距。通过对多个锚点的距离数据进行处理和计算，进一步实现了空间中的定位。这种方法在需要高精度和高可靠性的应用场景（如室内导航、资产跟踪等）中非常有用。
static void compute_angle_send_to_anthor0(int distance1, int distance2,int distance3)
static void compute_angle_send_to_anthor0(int distance1, int distance2,int distance3)
{
static int framenum = 0 ; // 静态变量framenum用于跟踪帧数或序列号
// 计算角度的部分代码（目前被条件编译指令注释掉）
#if 0 //compute angle for smartcar
float dis3_constans = DISTANCE3; // 定义第三个距离常量
float cos = 0; // 用于计算角度的余弦值
float angle = 0 ; // 最终计算出的角度值
float dis1 = (float)distance1/1000; // 将第一个距离转换为米
float dis2 = (float)distance2/1000; // 将第二个距离转换为米
// 用于确保距离值有效的检查
if(dis1 + dis3_constans < dis2 || dis2+dis3_constans < dis1)
{
}
// 使用余弦定理计算角度
cos = (dis1*dis1 + dis3_constans* dis3_constans - dis2*dis2)/(2*dis1*dis3_constans);
angle = acos(cos)*180/3.1415926; // 将计算结果转换为度
printf("cos = %f, arccos = %f\r\n",cos,angle); // 打印计算结果
sprintf(dist_str, "angle: %3.2f m", angle); // 将角度格式化为字符串
OLED_ShowString(0, 6," "); // 清除显示
OLED_ShowString(0, 6,dist_str); // 显示角度
// 根据角度值给出行动指示
if(dis1 > 1)
{
if(angle > 110)
{
printf("turn right\r\n"); // 角度大于110度，向右转
angle_msg[10] = 'R';
}
else if(angle < 75)
{
printf("turn left\r\n"); // 角度小于75度，向左转
angle_msg[10] = 'L';
}
else
{
printf("forward\r\n"); // 角度在这两者之间，直行
angle_msg[10] = 'F';
}
}
else
{
printf("stay here\r\n"); // 如果距离太近，就原地不动
angle_msg[10] = 'S';
}
angle_msg[LOCATION_FLAG_IDX] = 0;
#else
// 构建位置信息并发送
{
uint8 len = 0; // 定义长度变量
angle_msg[LOCATION_FLAG_IDX] = 1; // 设置位置标志位
// 以下是构建消息内容，包括各种位置信息和距离信息
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 'm';
// 添加字符'm'，可能表示特定的指令或标记
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 'r';
// 添加字符'r'，与上一行一起可能表示一个命令或状态
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 0x02;
// 添加特定的命令或标志0x02
angle_msg[LOCATION_INFO_START_IDX + (len++)] = TAG_ID;
// 添加TAG ID，用以区分不同的标签或设备
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)(framenum&0xFF);
// 添加帧序号的低8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((framenum>>8)&0xFF);
// 添加帧序号的高8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10)&0xFF);
// 添加第一个距离值的低8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10 >>8)&0xFF);
// 添加第一个距离值的高8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance2/10)&0xFF);
// 添加第二个距离值的低8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance2/10 >>8)&0xFF);
// 添加第二个距离值的高8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance3/10)&0xFF);
// 添加第三个距离值的低8位
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance3/10 >>8)&0xFF);
// 添加第三个距离值的高8位
// 如果有更多锚点，添加其他距离
if(ANCHOR_MAX_NUM > 3)
{
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((Anthordistance[3]/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((Anthordistance[3]/10 >>8)&0xFF);
}
else
{
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10 >>8)&0xFF);
}
angle_msg[LOCATION_INFO_START_IDX + (len++)] = '\n';
angle_msg[LOCATION_INFO_START_IDX + (len++)] = '\r';
angle_msg[LOCATION_INFO_LEN_IDX] = len; // 设置消息长度
// 检查消息长度是否超过最大值
if(LOCATION_INFO_START_IDX + len -2 >ANGLE_MSG_MAX_LEN)
{
while(1); // 如果超过最大长度，则进入无限循环（错误处理）
}
//USART_puts((char*)angle_msg,len); // 发送消息
}
#endif
// 设置消息的序列号和标签ID，并发送
angle_msg[ALL_MSG_SN_IDX] = framenum;
angle_msg[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(angle_msg), angle_msg, 0); // 准备数据传输
dwt_writetxfctrl(sizeof(angle_msg), 0); // 设置传输控制
/* 开始传输，并自动启用接收，等待传输完成后的延时 */
dwt_starttx(DWT_START_TX_IMMEDIATE );
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ }; // 等待传输完成
framenum++; // 更新帧数
}
这段代码是一个函数，名为compute_angle_send_to_anthor0，它的作用是根据提供的三个距离参数（distance1, distance2, distance3），计算与某个目标之间的角度，或者发送包含位置信息的消息。这个函数内部有两部分主要的逻辑，通过预处理指令#if 0控制哪部分代码是激活的。让我们逐步解释这个函数的不同部分：
注释掉的计算角度部分（当#if 0）:
-
初始化变量:
framenum用于记录消息的帧序号，每次函数被调用时自增。dis1和dis2是将距离从毫米转换为米的结果。dis3_constans是第三个距离常数，也是以米为单位。
-
计算角度:
- 使用三角形的余弦定理来计算两个设备之间的角度。
- 计算结果
angle是以度为单位的角度值。
-
根据角度执行动作:
- 根据计算出的角度，判断是应该向左转、向右转、直行或停止，并设置
angle_msg[10]为相应的指令（‘L’, ‘R’, ‘F’, ‘S’）。
- 根据计算出的角度，判断是应该向左转、向右转、直行或停止，并设置
-
设置位置标志:
angle_msg[LOCATION_FLAG_IDX] = 0;表示当前消息不包含位置信息。
当#if 1时，即发送位置信息部分:
-
填充消息:
- 这部分代码构造一个包含位置信息的消息。它首先设置一个位置标志，然后依次添加各种信息到
angle_msg数组中。
- 这部分代码构造一个包含位置信息的消息。它首先设置一个位置标志，然后依次添加各种信息到
-
消息内容:
- 'mr’可能是消息的一个标识符或类型。
0x02可能表示版本号或消息类型。TAG_ID是标签的ID。framenum是当前消息的帧编号。distance1/10,distance2/10,distance3/10是距离数据，以某种格式缩小10倍后加入消息。
-
消息结束处理:
- 添加换行符和回车符作为消息的结束。
- 检查消息长度，确保不超过最大长度
ANGLE_MSG_MAX_LEN。
-
发送消息:
- 设置消息的序列号和标签ID。
- 调用
dwt_writetxdata和dwt_writetxfctrl来配置发送的数据和控制参数。 - 调用
dwt_starttx来开始发送数据，并等待发送完成。
总结:
这个函数是为了发送特定格式的数据包。在注释部分，它计算一个角度并决定如何移动，可能用于控制一个智能车辆或类似的设备。在未注释部分，它构建包含多个距离信息的位置数据包，并发送出去，可能用于定位或跟踪系统。#if 0和#else部分表明了代码的两种不同用途或模式，但一次只能使用一种。实际应用中根据需要选择适当的部分，并可能需要进一步调整以适应特定的硬件和应用要求。
int filter(int input, int fliter_idx )
int filter(int input, int fliter_idx )
{
char count = 0;
int sum = 0;
if(input > 0)
{
Value_Buf[fliter_idx][filter_index[fliter_idx]++]=input;
if(filter_index[fliter_idx] == Filter_D) filter_index[fliter_idx] = 0;
for(count = 0; count<Filter_D; count++)
{
sum += Value_Buf[fliter_idx][count];
}
return (int)(sum/Filter_D);
}
else
{
for(count = 0; count<Filter_D; count++)
{
sum += Value_Buf[fliter_idx][count];
}
return (int)(sum/Filter_D);
}
}
这段代码定义了一个名为 filter 的函数，它实现了一个简单的滤波器功能。该函数接收两个参数：一个输入值 input 和一个滤波器索引 filter_idx。函数的主要目的是对一系列输入值进行平均，以此作为滤波处理。这里是代码的详细解释：
-
函数签名:
int filter(int input, int filter_idx):
这表示filter是一个接受两个整数参数的函数，input代表输入值，filter_idx代表当前使用的滤波器的索引。
-
变量声明:
char count = 0;: 声明并初始化一个字符类型的计数器count。int sum = 0;: 声明并初始化一个整数sum，用于累加滤波器的值。
-
输入值的处理:
if(input > 0): 判断输入值是否大于0。- 如果是，将输入值添加到
Value_Buf[filter_idx]数组的当前索引位置，然后增加filter_index[filter_idx]。 if(filter_index[filter_idx] == Filter_D) filter_index[filter_idx] = 0;: 如果当前滤波器索引等于Filter_D（一个未在代码中定义的常量，可能代表滤波器的大小），则将索引重置为0。for(count = 0; count < Filter_D; count++): 循环遍历滤波器数组。sum += Value_Buf[filter_idx][count];: 将滤波器数组中的值累加到sum。return (int)(sum / Filter_D);: 返回sum除以Filter_D的结果，这实际上是求输入值的平均值。
- 如果是，将输入值添加到
-
处理负输入值:
else: 如果输入值不大于0。- 与上面的循环相同，它也计算
Value_Buf[filter_idx]数组值的平均数，并返回这个平均值。
- 与上面的循环相同，它也计算
-
返回值:
- 无论输入值是正是负，函数都返回滤波器数组的当前平均值。
需要注意的是，代码中使用了几个未在函数内部定义的外部变量，如 Value_Buf、filter_index 和 Filter_D。这些变量可能在函数外部定义，代表滤波器的存储结构和相关参数。此外，函数对输入值的正负进行了不同的处理，但最终都执行了相同的平均值计算操作。
trilateration.c
矩阵运算
// 定义一个vec3d结构体，用于表示三维向量
typedef struct vec3d vec3d;
struct vec3d {
double x; // x坐标
double y; // y坐标
double z; // z坐标
};
// 定义一个用于计算两个三维向量差的函数
vec3d vdiff(const vec3d vector1, const vec3d vector2)
{
vec3d v; // 定义一个新的三维向量v
// 计算第一个向量和第二个向量在x轴的差
v.x = vector1.x - vector2.x;
// 计算第一个向量和第二个向量在y轴的差
v.y = vector1.y - vector2.y;
// 计算第一个向量和第二个向量在z轴的差
v.z = vector1.z - vector2.z;
// 返回计算得到的向量差
return v;
}
// 定义一个用于计算两个三维向量和的函数
vec3d vsum(const vec3d vector1, const vec3d vector2)
{
vec3d v; // 定义一个新的三维向量v
// 计算第一个向量和第二个向量在x轴的和
v.x = vector1.x + vector2.x;
// 计算第一个向量和第二个向量在y轴的和
v.y = vector1.y + vector2.y;
// 计算第一个向量和第二个向量在z轴的和
v.z = vector1.z + vector2.z;
// 返回计算得到的向量和
return v;
}
// 定义一个用于将三维向量乘以一个数的函数
vec3d vmul(const vec3d vector, const double n)
{
vec3d v; // 定义一个新的三维向量v
// 将向量在x轴上的分量乘以n
v.x = vector.x * n;
// 将向量在y轴上的分量乘以n
v.y = vector.y * n;
// 将向量在z轴上的分量乘以n
v.z = vector.z * n;
// 返回计算得到的向量
return v;
}
// 定义一个用于将三维向量除以一个数的函数
vec3d vdiv(const vec3d vector, const double n)
{
vec3d v; // 定义一个新的三维向量v
// 将向量在x轴上的分量除以n
v.x = vector.x / n;
// 将向量在y轴上的分量除以n
v.y = vector.y / n;
// 将向量在z轴上的分量除以n
v.z = vector.z / n;
// 返回计算得到的向量
return v;
}
// 定义一个计算两个三维向量之间距离的函数
double vdist(const vec3d v1, const vec3d v2)
{
// 计算两个向量在x轴上的差值
double xd = v1.x - v2.x;
// 计算两个向量在y轴上的差值
double yd = v1.y - v2.y;
// 计算两个向量在z轴上的差值
double zd = v1.z - v2.z;
// 返回两个向量之间的欧几里得距离
return sqrt(xd * xd + yd * yd + zd * zd);
}
// 定义一个计算三维向量的范数（长度）的函数
double vnorm(const vec3d vector)
{
// 返回向量的欧几里得范数（即向量的长度）
return sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
}
// 定义一个计算两个三维向量点积的函数
double dot(const vec3d vector1, const vec3d vector2)
{
// 计算并返回两个向量的点积
return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
}
// 定义一个计算两个三维向量叉积的函数
vec3d cross(const vec3d vector1, const vec3d vector2)
{
vec3d v; // 定义一个新的三维向量v
// 计算在y轴和z轴上分量的乘积差，作为新向量的x分量
v.x = vector1.y * vector2.z - vector1.z * vector2.y;
// 计算在z轴和x轴上分量的乘积差，作为新向量的y分量
v.y = vector1.z * vector2.x - vector1.x * vector2.z;
// 计算在x轴和y轴上分量的乘积差，作为新向量的z分量
v.z = vector1.x * vector2.y - vector1.y * vector2.x;
// 返回计算得到的向量
return v;
}
double gdoprate(const vec3d tag, const vec3d p1, const vec3d p2, const vec3d p3)
// 定义一个计算地理定位系统中几何精度因子（GDOP）的函数
double gdoprate(const vec3d tag, const vec3d p1, const vec3d p2, const vec3d p3)
{
vec3d ex, t1, t2, t3; // 定义用于计算的三维向量
double h, gdop1, gdop2, gdop3, result; // 定义计算中使用的变量
// 计算tag到p1的单位向量
ex = vdiff(p1, tag); // 计算差向量
h = vnorm(ex); // 计算差向量的范数
t1 = vdiv(ex, h); // 归一化向量
// 计算tag到p2的单位向量
ex = vdiff(p2, tag); // 同上
h = vnorm(ex); // 同上
t2 = vdiv(ex, h); // 同上
// 计算tag到p3的单位向量
ex = vdiff(p3, tag); // 同上
h = vnorm(ex); // 同上
t3 = vdiv(ex, h); // 同上
// 计算各个向量之间的点积的绝对值
gdop1 = fabs(dot(t1, t2));
gdop2 = fabs(dot(t2, t3));
gdop3 = fabs(dot(t3, t1));
// 找出最大的GDOP值
if (gdop1 < gdop2) result = gdop2;
else result = gdop1;
if (result < gdop3) result = gdop3;
// 返回最大的GDOP值
return result;
}
这段代码定义了一个用于计算地理定位系统中几何精度因子（GDOP）的函数。GDOP是一种用于衡量全球导航卫星系统（如GPS）中位置解的精度的指标。它表示接收器所在位置和卫星的几何配置对定位精度的影响。GDOP越小，定位精度越高。
下面是代码的详细解释：
-
double gdoprate(const vec3d tag, const vec3d p1, const vec3d p2, const vec3d p3)：这是函数的定义，它接受四个参数，分别是tag（接收器的位置）和三个卫星的位置（p1，p2，p3）。 -
vec3d ex, t1, t2, t3;：定义了四个三维向量，ex用于临时存储差向量，t1，t2，t3用于存储单位向量。 -
double h, gdop1, gdop2, gdop3, result;：定义了一些临时变量，h用于存储向量的范数，gdop1，gdop2，gdop3用于存储计算得到的GDOP值，result用于存储最终的GDOP结果。 -
计算
tag到p1的单位向量：ex = vdiff(p1, tag);：计算差向量，即从tag指向p1的向量。h = vnorm(ex);：计算差向量的范数，即长度。t1 = vdiv(ex, h);：将差向量归一化，得到单位向量t1。
-
类似地，计算
tag到p2和tag到p3的单位向量，分别存储在t2和t3中。 -
计算各个单位向量之间的点积的绝对值，这是计算GDOP的关键：
gdop1 = fabs(dot(t1, t2));：计算t1和t2的点积的绝对值，存储在gdop1中。gdop2 = fabs(dot(t2, t3));：计算t2和t3的点积的绝对值，存储在gdop2中。gdop3 = fabs(dot(t3, t1));：计算t3和t1的点积的绝对值，存储在gdop3中。
-
找出最大的GDOP值：
if (gdop1 < gdop2) result = gdop2;：比较gdop1和gdop2的大小，将较大的值存储在result中。if (result < gdop3) result = gdop3;：再次比较result和gdop3的大小，将较大的值存储在result中。现在，result包含了三个GDOP值中的最大值。
-
最后，函数返回最大的GDOP值
result。
总之，这个函数用于计算给定接收器位置和三个卫星位置的情况下的GDOP值，以评估定位精度。GDOP越小，表示卫星的几何配置对定位精度的影响越小，从而定位精度越高。
GDOP
GDOP（Geometric Dilution of Precision，几何精度衰减）是一个用于描述卫星定位系统中位置测量精度受卫星几何排布影响程度的量度。它是全球定位系统（GPS）和其他类似卫星导航系统（如GLONASS、Galileo等）的重要概念。GDOP 值越小，表明定位系统的精度越高。下面详细解释GDOP的各个方面：
基本概念
-
定义：GDOP 是一种量化卫星几何配置对定位测量精度影响的指标。它基于你的接收器与卫星之间的相对位置关系。
-
组成：GDOP 是几个不同类型的DOP（Dilution of Precision，精度衰减）指标的组合，主要包括：
- PDOP（位置精度衰减）：影响三维位置（纬度、经度和高度）测量的精度。
- HDOP（水平精度衰减）：影响水平位置（纬度和经度）测量的精度。
- VDOP（垂直精度衰减）：影响垂直位置（高度）测量的精度。
- TDOP（时间精度衰减）：影响时间测量的精度。
计算和影响因素
-
计算：GDOP 的计算涉及确定接收器与多颗卫星之间的相对角度。理想情况下，这些卫星应分布在接收器的不同方向，以减少位置测量的不确定性。
-
影响因素：
- 卫星数量：可用卫星的数量越多，通常可以获得更好的GDOP值。
- 卫星分布：卫星在天空中的分布越均匀，GDOP值通常越低。
- 地理和环境因素：建筑物、山脉等障碍物可能影响卫星信号，从而影响GDOP。
GDOP 与定位精度
- 低GDOP值：表示卫星的几何排布对定位精度的负面影响较小，定位结果更加精确。
- 高GDOP值：表示卫星的几何排布对定位精度有较大的负面影响，定位结果的不确定性增大。
实际应用
在GPS和其他卫星导航系统中，GDOP通常用来评估在特定时间和位置的定位精度。导航设备和软件可能会使用GDOP值来决定是否接受当前的定位数据，或者在寻找更好的卫星配置以获取更精确的定位信息。
int sphereline(const vec3d p1, const vec3d p2, const vec3d sc, double r, double *const mu1, double *const mu2)
int sphereline(const vec3d p1, const vec3d p2, const vec3d sc, double r, double *const mu1, double *const mu2)
{
double a,b,c; // 声明用于计算的系数变量a, b, c
double bb4ac; // 声明用于存储判别式的变量
vec3d dp; // 声明用于存储差向量的变量
dp.x = p2.x - p1.x; // 计算点p2和p1在x轴上的差值
dp.y = p2.y - p1.y; // 计算点p2和p1在y轴上的差值
dp.z = p2.z - p1.z; // 计算点p2和p1在z轴上的差值
a = dp.x * dp.x + dp.y * dp.y + dp.z * dp.z; // 计算系数a（差向量的长度的平方）
b = 2 * (dp.x * (p1.x - sc.x) + dp.y * (p1.y - sc.y) + dp.z * (p1.z - sc.z)); // 计算系数b
c = sc.x * sc.x + sc.y * sc.y + sc.z * sc.z; // 计算系数c的第一部分
c += p1.x * p1.x + p1.y * p1.y + p1.z * p1.z; // 加上系数c的第二部分
c -= 2 * (sc.x * p1.x + sc.y * p1.y + sc.z * p1.z); // 减去系数c的第三部分
c -= r * r; // 减去半径的平方
bb4ac = b * b - 4 * a * c; // 计算判别式
if (fabs(a) == 0 || bb4ac < 0) // 检查是否有交点
{
*mu1 = 0; // 将mu1设为0
*mu2 = 0; // 将mu2设为0
return -1; // 返回-1，表示没有交点
}
*mu1 = (-b + sqrt(bb4ac)) / (2 * a); // 计算第一个交点参数
*mu2 = (-b - sqrt(bb4ac)) / (2 * a); // 计算第二个交点参数
return 0; // 返回0，表示成功计算了交点
}
这段代码定义了一个名为 sphereline 的函数，用于计算一条直线与一个球体的交点。直线由两个三维点 p1 和 p2 定义，球体由中心 sc 和半径 r 定义。如果直线与球体相交，函数将计算两个交点并将它们的参数值存储在 mu1 和 mu2 中。下面是代码的逐行解释：
-
函数签名：
int sphereline(const vec3d p1, const vec3d p2, const vec3d sc, double r, double *const mu1, double *const mu2)：
这表示sphereline是一个接受六个参数的函数，包括两个点p1和p2（定义直线），球体中心sc，球体半径r，以及两个用于存储交点参数的指针mu1和mu2。
-
变量声明：
double a,b,c;：用于存储计算中的系数。double bb4ac;：用于存储判别式的值。vec3d dp;：用于存储点p1和p2之间的差向量。
3-6. 计算差向量 dp：
- 这几行代码计算
p1和p2之间的差向量，即直线的方向向量。
-
计算系数
a：a是差向量dp的点积，代表直线方向向量的长度的平方。
-
计算系数
b：b是直线方向向量与从球心到直线起点的向量的点积的两倍。
9-12. 计算系数 c：
c的计算涉及球心坐标、直线起点坐标、球半径，并根据球体方程得出。
- 计算判别式
bb4ac：
bb4ac = b * b - 4 * a * c：这是求解二次方程时用到的判别式，用于判断方程是否有实数解（即直线是否与球相交）。
14-20. 检查交点的存在性：
- 如果
a接近于零（直线方向向量的长度几乎为零）或bb4ac小于零（方程没有实数解），则将mu1和mu2设为零，并返回-1表示没有交点。
21-22. 计算交点参数：
- 如果存在交点，使用二次方程的求根公式计算两个解
mu1和mu2。
- 返回值：
- 返回
0表示成功计算了交点。
总之，这个函数通过求解二次方程来找出直线与球体的交点。交点的存在性通过判别式确定，交点的参数通过二次方程的根计算得出。这种类型的计算在计算几何和图形处理中非常常见。
int GetLocation(vec3d best_solution, int use4thAnchor, vec3d anchorArray, int *distanceArray)
int GetLocation(vec3d *best_solution, int use4thAnchor, vec3d* anchorArray, int *distanceArray)
{
// 定义变量用于存储两个可能的解和四个锚点的坐标
vec3d o1, o2, p1, p2, p3, p4;
// 定义四个距离和最优误差以及几何稀疏度（GDOP）
double r1 = 0, r2 = 0, r3 = 0, r4 = 0, best_3derror, best_gdoprate;
// 定义结果和错误代码以及组合计数器
int result;
int error, combination;
// 临时变量用于存储计算过程中的距离
vec3d t3;
double dist1, dist2;
// 提取锚点数组中的坐标
p1.x = anchorArray[0].x;
p1.y = anchorArray[0].y;
p1.z = anchorArray[0].z;
p2.x = anchorArray[1].x;
p2.y = anchorArray[1].y;
p2.z = anchorArray[1].z;
p3.x = anchorArray[2].x;
p3.y = anchorArray[2].y;
p3.z = anchorArray[2].z;
// 第四个坐标被设置为第一个坐标，表示只使用三个锚点进行定位
p4.x = anchorArray[0].x;
p4.y = anchorArray[0].y;
p4.z = anchorArray[0].z;
// 距离数组转换为米
r1 = (double)distanceArray[0] / 1000.0;
r2 = (double)distanceArray[1] / 1000.0;
r3 = (double)distanceArray[2] / 1000.0;
// 第四个距离同第一个，因为假设只用到三个锚点进行定位
r4 = (double)distanceArray[0] / 1000.0;
// 打印距离和锚点坐标信息，用于调试
printf("r1=%f , r2=%f, r3=%f, r4=%f\n", r1, r2, r3, r4);
printf("Anchor0: x = %f, y = %f\n", p1.x, p1.y);
printf("Anchor1: x = %f, y = %f\n", p2.x, p2.y);
printf("Anchor2: x = %f, y = %f\n", p3.x, p3.y);
// 使用三边测量（trilateration）算法计算最佳位置
result = deca_3dlocate(&o1, &o2, best_solution, &error, &best_3derror, &best_gdoprate,
p1, r1, p2, r2, p3, r3, p4, r4, &combination);
// 打印结果
printf("result = %d\n", result);
// 如果结果有效
if(result >= 0)
{
// 如果使用第四个锚点进行定位
if (use4thAnchor == 1)
{
double diff1, diff2;
// 计算两个解决方案到第四个锚点的距离
t3 = vdiff(o1, anchorArray[3]);
dist1 = vnorm(t3);
t3 = vdiff(o2, anchorArray[3]);
dist2 = vnorm(t3);
// 找出与第四个锚点距离最接近的解
diff1 = fabs(r4 - dist1);
diff2 = fabs(r4 - dist2);
if (diff1 < diff2) *best_solution = o1;
else *best_solution = o2;
}
else
{
// 如果不使用第四个锚点，假设目标位于所有锚点的下方，并选择z值较小的解作为最佳解
if(o1.z < p1.z) *best_solution = o1;
else *best_solution = o2;
}
}
// 返回定位结果，如果成功返回计算结果，否则返回错误代码-1
if (result >= 0)
{
return result;
}
//return error
return -1;
}
这段代码是一个三维定位函数，用于基于三个或四个锚点的距离测量结果来确定一个对象的位置。这种方法常用于无线定位系统，例如室内定位。下面逐步解释代码的功能：
-
函数定义：
GetLocation是函数名。vec3d *best_solution是一个指针，指向vec3d结构，用于存储计算出的最佳位置。int use4thAnchor是一个标志，指示是否使用第四个锚点来改进定位结果。vec3d* anchorArray是一个数组，包含了锚点的坐标。int *distanceArray是一个数组，包含了从各锚点到对象的距离测量结果。
-
初始化变量：
- 定义了多个
vec3d类型的变量来存储锚点坐标和两个可能的解。 - 定义了
double类型的变量来存储测距结果。 - 初始化了几个用于存储计算结果的变量。
- 定义了多个
-
锚点坐标的设置：
- 从
anchorArray中读取前三个锚点的坐标。 - 第四个坐标被设置成与第一个锚点相同，因为这里只使用三个锚点进行三边测量。
- 从
-
测距结果的转换：
- 从
distanceArray中读取测距结果，并将其从毫米转换为米。
- 从
-
调用定位函数：
deca_3dlocate是一个外部定义的函数，用于计算对象的位置。函数返回两个可能的位置（o1和o2），计算误差和质量指标（best_3derror和best_gdoprate），并选择最佳解。
-
选择最佳解：
- 如果使用第四个锚点，函数将比较两个解与第四个锚点的距离，并选择与第四个锚点测距结果最匹配的解。
- 如果不使用第四个锚点，函数假设对象位于所有锚点下方，并选择一个z坐标小于第一个锚点z坐标的解。
-
返回结果：
- 如果
deca_3dlocate成功计算出位置，函数返回计算结果。 - 如果计算失败，函数返回
-1表示错误。
- 如果
这段代码的目标是利用测距数据来解决一个典型的三维定位问题。代码中可能省略了一些细节，例如 deca_3dlocate 函数的实现和 vec3d 结构的定义，但其核心逻辑是根据给定的距离数据和锚点位置计算对象的最佳三维坐标。
int deca_3dlocate(……)
int deca_3dlocate ( vec3d *const solution1,
vec3d *const solution2,
vec3d *const best_solution,
int *const nosolution_count,
double *const best_3derror,
double *const best_gdoprate,
vec3d p1, double r1,
vec3d p2, double r2,
vec3d p3, double r3,
vec3d p4, double r4,
int *combination)
{
// 定义两个可能的解决方案和临时变量
vec3d o1, o2, solution, ptemp;
vec3d solution_compare1, solution_compare2;
double rtemp;
double gdoprate_compare1, gdoprate_compare2;
// 定义覆盖半径和计数器变量
double ovr_r1, ovr_r2, ovr_r3, ovr_r4;
int overlook_count, combination_counter;
// 定义三边测量错误计数器和模式变量
int trilateration_errcounter, trilateration_mode34;
int success, concentric, result;
// 初始化三边测量错误计数器和模式变量
trilateration_errcounter = 0;
trilateration_mode34 = 0;
// 初始化球体组合计数器
combination_counter = 4; // 四个球体的组合
// 设置最差的几何稀疏度（GDOP）初始值
*best_gdoprate = 1;
gdoprate_compare1 = 1;
gdoprate_compare2 = 1;
solution_compare1.x = 0;
solution_compare1.y = 0;
solution_compare1.z = 0;
// 进行循环尝试不同的球体组合以找到解决方案
do
{
// 初始化成功和同心圆标志，以及覆盖计数器
success = 0;
concentric = 0;
overlook_count = 0;
ovr_r1 = r1;
ovr_r2 = r2;
ovr_r3 = r3;
ovr_r4 = r4;
// 尝试三边测量计算，不同的返回值代表不同的情况
do
{
result = trilateration(&o1, &o2, &solution, p1, ovr_r1, p2, ovr_r2, p3, ovr_r3, p4, ovr_r4, MAXZERO);
switch (result)
{
case TRIL_3SPHERES: // 使用三个球体得到结果
trilateration_mode34 = TRIL_3SPHERES;
success = 1;
break;
case TRIL_4SPHERES: // 使用四个球体得到结果
trilateration_mode34 = TRIL_4SPHERES;
success = 1;
break;
case ERR_TRIL_CONCENTRIC: // 锚点是同心的，无法计算
concentric = 1;
break;
default: // 其他返回值，增加半径重试
ovr_r1 += 0.10;
ovr_r2 += 0.10;
ovr_r3 += 0.10;
ovr_r4 += 0.10;
overlook_count++;
break;
}
}
while (!success && (overlook_count <= 5) && !concentric);
// 如果成功找到解决方案，根据三边测量结果进行处理
if (success)
{
switch (result)
{
case TRIL_3SPHERES: // 如果使用三个球体
*solution1 = o1;
*solution2 = o2;
*nosolution_count = overlook_count;
// 重置组合计数器
combination_counter = 0;
break;
case TRIL_4SPHERES: // 如果使用四个球体
// 计算新的几何稀疏度（GDOP）
gdoprate_compare1 = gdoprate(solution, p1, p2, p3);
// 与之前的结果进行比较，并选择更好的结果
if (gdoprate_compare1 <= gdoprate_compare2)
{
*solution1 = o1;
*solution2 = o2;
*best_solution = solution;
*nosolution_count = overlook_count;
// 计算最佳三维误差
*best_3derror = sqrt((vnorm(vdiff(solution, p1))-r1)*(vnorm(vdiff(solution, p1))-r1) +
(vnorm(vdiff(solution, p2))-r2)*(vnorm(vdiff(solution, p2))-r2) +
(vnorm(vdiff(solution, p3))-r3)*(vnorm(vdiff(solution, p3))-r3) +
(vnorm(vdiff(solution, p4))-r4)*(vnorm(vdiff(solution, p4))-r4));
// 更新几何稀疏度（GDOP）
*best_gdoprate = gdoprate_compare1;
// 保存之前的结果
solution_compare2 = solution_compare1;
gdoprate_compare2 = gdoprate_compare1;
// 更新组合计数器
*combination = 5 - combination_counter;
// 循环使用不同的锚点组合
ptemp = p1; p1 = p2; p2 = p3; p3 = p4; p4 = ptemp;
rtemp = r1; r1 = r2; r2 = r3; r3 = r4; r4 = rtemp;
combination_counter--;
}
break;
default:
break;
}
}
else
{
// 如果未找到解决方案，更新错误计数器和组合计数器
trilateration_errcounter = 4;
combination_counter = 0;
}
}
while (combination_counter);
// 如果所有四个球体的组合都无法得到有效结果，则返回错误
if (trilateration_errcounter >= 4) return -1;
else return trilateration_mode34; // 否则返回使用的三边测量模式
}
这段代码是 deca_3dlocate 函数的实现，它尝试使用四个球体（通常是无线信标或锚点）来定位一个点（通常是标签）在三维空间中的位置。这个过程被称为三边测量（trilateration），是定位系统中常见的技术。这个特定的函数实现了一个迭代过程，尝试找到最佳的解决方案，并考虑多种不同的球体组合。以下是对该函数关键部分的详细解释：
-
函数接受多个参数，包括指向解决方案向量的指针、锚点的位置、测量到的距离以及用于记录特定计算组合的变量。
-
函数内部定义了多个局部变量用于存储中间计算结果和状态。
-
trilateration_errcounter和trilateration_mode34用于记录错误次数和选择的三边测量模式。 -
combination_counter用于追踪不同的球体组合。 -
do...while循环是函数的主体，它尝试使用不同的球体组合来找到位置解。 -
trilateration函数被调用来计算可能的两个解决方案（o1和o2）和一个最佳解决方案（solution）。这个函数的实现细节没有在这段代码中提供。 -
根据
trilateration函数的返回值，会有几种情况处理：- 如果成功找到了一个基于三个或四个球体的解决方案，它会存储在
solution1和solution2中。 - 如果所有球体组合都不能提供有效的结果，函数最终返回
-1表示失败。
- 如果成功找到了一个基于三个或四个球体的解决方案，它会存储在
-
如果使用四个球体成功找到解决方案，函数计算几何稀疏度（GDOP，一个表示解决方案质量的指标），并将最佳解决方案存储在
best_solution中。 -
迭代过程包括轻微改变球体的半径，这可能是为了解决计算中的问题，比如锚点可能在现实世界中不完全是几何中心对称的。
-
代码中的某些部分被注释掉了，这些通常是用于调试的语句，例如
qDebug()。
总的来说，这个函数的目标是通过迭代地尝试不同的球体组合来找到一个稳健的位置解。这在UWB定位系统中很常见，其中测量精度非常高，但是受到环境因素和信号反射的影响。通过迭代和比较不同的解决方案，可以提高定位的准确性和可靠性。
int trilateration(……)
int trilateration(vec3d *const result1,
vec3d *const result2,
vec3d *const best_solution,
const vec3d p1, const double r1,
const vec3d p2, const double r2,
const vec3d p3, const double r3,
const vec3d p4, const double r4,
const double maxzero)
{
vec3d ex, ey, ez, t1, t2, t3; // 定义三维向量ex, ey, ez和临时向量t1, t2, t3
double h, i, j, x, y, z, t; // 定义用于计算的临时变量
double mu1, mu2, mu; // 定义用于后续计算的变量
int result; // 定义函数返回结果的变量
// 从前三个球体中找到两个交点
// 检查前三个球体是否有至少两个同心的球体
// 如果有，则无法继续计算，返回错误-1
/* h = |p3 - p1|, ex = (p3 - p1) / |p3 - p1| */
ex = vdiff(p3, p1); // 计算向量p3到p1
h = vnorm(ex); // 计算向量的模长
if (h <= maxzero)
{
// 如果p1和p3同心，则无法获取精确的交点
return ERR_TRIL_CONCENTRIC;
}
/* h = |p3 - p2|, ex = (p3 - p2) / |p3 - p2| */
ex = vdiff(p3, p2); // 计算向量p3到p2
h = vnorm(ex); // 计算向量的模长
if (h <= maxzero)
{
// 如果p2和p3同心，则无法获取精确的交点
return ERR_TRIL_CONCENTRIC;
}
/* h = |p2 - p1|, ex = (p2 - p1) / |p2 - p1| */
ex = vdiff(p2, p1); // 计算向量p2到p1
h = vnorm(ex); // 计算向量的模长
if (h <= maxzero)
{
// 如果p1和p2同心，则无法获取精确的交点
return ERR_TRIL_CONCENTRIC;
}
/* Is point p1 + (r1 along the axis) the intersection? */
t2 = vsum(p1, vmul(ex, r1)); // 检查点p1沿着r1方向的点是否为交点
if (fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero &&
fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero)
{
/* Yes, t2 is the only intersection point. */
if (result1) *result1 = t2; // 是的，t2是唯一的交点
if (result2) *result2 = t2;
return TRIL_3SPHERES; // 返回表示三个球面交点的结果
}
/* Is point p1 - (r1 along the axis) the intersection? */
t2 = vsum(p1, vmul(ex, -r1)); // 检查点p1沿着-r1方向的点是否为交点
if (fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero &&
fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero)
{
/* Yes, t2 is the only intersection point. */
if (result1) *result1 = t2; // 是的，t2是唯一的交点
if (result2) *result2 = t2;
return TRIL_3SPHERES; // 返回表示三个球面交点的结果
}
/* p1, p2 and p3 are colinear with more than one solution */
return ERR_TRIL_COLINEAR_2SOLUTIONS; // p1, p2, p3共线，存在多个解
/* ez = ex x ey */
ez = cross(ex, ey); // 计算ez为ex和ey的叉积，得到垂直于ex和ey的单位向量
// 接下来的计算用于求解交点
// ...
// 计算x, y, z坐标
x = (r1*r1 - r2*r2) / (2*h) + h / 2;
y = (r1*r1 - r3*r3 + i*i) / (2*j) + j / 2 - x * i / j;
z = r1*r1 - x*x - y*y;
if (z < -maxzero-100)
{
// 解决方案无效，负数的平方根
return ERR_TRIL_SQRTNEGNUMB;
}
else if (z > 0.0)
z = sqrt(z);
else
z = 0.0;
/* t2 = p1 + x ex + y ey */
t2 = vsum(p1, vmul(ex, x)); // 计算点p1加上x方向上的ex和y方向上的ey
t2 = vsum(t2, vmul(ey, y)); // 更新t2为p1加上x方向和y方向的位移
/* result1 = p1 + x ex + y ey + z ez */
if (result1)
*result1 = vsum(t2, vmul(ez, z)); // 如果result1非空，则设置result1为计算得到的第一个交点
/* result1 = p1 + x ex + y ey - z ez */
if (result2)
*result2 = vsum(t2, vmul(ez, -z)); // 如果result2非空，则设置result2为计算得到的第二个交点
/*********** 完成从前三个球中找两个点的计算 **********/
/********* RESULT1 AND RESULT2 ARE SOLUTIONS, OTHERWISE RETURN ERROR *********/
/************* 通过引入第四个球体来找到一个解决方案 ***********/
// 检查第四个球体与前三个球体是否同心
// 如果它与其中一个球体同心，则第四个球体不能用来确定最佳解决方案
/* h = |p4 - p1|, ex = (p4 - p1) / |p4 - p1| */
ex = vdiff(p4, p1); // 计算向量p4到p1
h = vnorm(ex); // 计算向量长度
if (h <= maxzero)
{
/* p1 和 p4 是同心的，无法获得精确的交点 */
return TRIL_3SPHERES;
}
/* h = |p4 - p2|, ex = (p4 - p2) / |p4 - p2| */
ex = vdiff(p4, p2); // 计算向量p4到p2
h = vnorm(ex); // 计算向量长度
if (h <= maxzero)
{
/* p2 和 p4 是同心的，无法获得精确的交点 */
return TRIL_3SPHERES;
}
/* h = |p4 - p3|, ex = (p4 - p3) / |p4 - p3| */
ex = vdiff(p4, p3); // 计算向量p4到p3
h = vnorm(ex); // 计算向量长度
if (h <= maxzero)
{
/* p3 和 p4 是同心的，无法获得精确的交点 */
return TRIL_3SPHERES;
}
// 如果第四个球体不与任何球体同心，可以获得最佳解决方案
// 计算result1到p4的距离
t3 = vdiff(*result1, p4);
i = vnorm(t3);
// 计算result2到p4的距离
t3 = vdiff(*result2, p4);
h = vnorm(t3);
// 选取离球体p4中心最近的点作为最佳解决方案
if (i > h)
{
*best_solution = *result1;
*result1 = *result2;
*result2 = *best_solution;
}
int count4 = 0;
double rr4 = r4;
result = 1;
/* 与第四个球的交点检查 */
while(result && count4 < 10)
{
result = sphereline(*result1, *result2, p4, rr4, &mu1, &mu2);
rr4 += 0.1; // 增加球半径以寻找交点
count4++;
}
if (result)
{
/* 没有找到result1-result2线段与球4的交点 */
*best_solution = *result1; // result1是离球4更近的解
}
else
{
if (mu1 < 0 && mu2 < 0)
{
/* 如果mu1和mu2都小于0 */
/* result1-result2线段在球4外部，没有交点 */
mu = (fabs(mu1) <= fabs(mu2)) ? mu1 : mu2;
/* 计算交点 */
ex = vdiff(*result2, *result1); // result1-result2向量
h = vnorm(ex); // 向量长度
ex = vdiv(ex, h); // 单位化
mu = 0.5 * mu; // 错误校正
t2 = vmul(ex, mu * h);
t2 = vsum(*result1, t2);
*best_solution = t2; // 最佳解决方案
}
else if ((mu1 < 0 && mu2 > 1) || (mu2 < 0 && mu1 > 1))
{
/* 一个mu值在0到1之间，另一个不在 */
/* result1-result2线段在球4内部，没有交点 */
mu = (mu1 > mu2) ? mu1 : mu2;
ex = vdiff(*result2, *result1); // result1-result2向量
h = vnorm(ex); // 向量长度
ex = vdiv(ex, h); // 单位化
t2 = vmul(ex, mu * h);
t2 = vsum(*result1, t2);
t3 = vmul(vdiff(*result2, t2), 0.5);
*best_solution = vsum(t2, t3); // 最佳解决方案
}
else if (((mu1 > 0 && mu1 < 1) && (mu2 < 0 || mu2 > 1))
|| ((mu2 > 0 && mu2 < 1) && (mu1 < 0 || mu1 > 1)))
{
/* 如果一个mu在0到1之间而另一个不在 */
/* result1-result2线段与球4在一个点上相交 */
mu = (mu1 >= 0 && mu1 <= 1) ? mu1 : mu2;
/* 为mu添加或减去0.5*mu，以便在每个球上平均分配错误 */
if (mu <= 0.5) mu -= 0.5 * mu;
else mu -= 0.5 * (1 - mu);
/* 计算交点 */
ex = vdiff(*result2, *result1); // result1-result2向量
h = vnorm(ex); // 向量长度
ex = vdiv(ex, h); // 单位化
t2 = vmul(ex, mu * h);
t2 = vsum(*result1, t2);
*best_solution = t2; // 最佳解决方案
}
else if (mu1 == mu2)
{
/* 如果mu1和mu2相等且都在0到1之间 */
/* result1-result2线段在球4上与之切线 */
mu = mu1;
/* 为mu添加或减去0.5*mu，以便在每个球上平均分配错误 */
if (mu <= 0.25) mu -= 0.5 * mu;
else if (mu <= 0.5) mu -= 0.5 * (0.5 - mu);
else if (mu <= 0.75) mu -= 0.5 * (mu - 0.5);
else mu -= 0.5 * (1 - mu);
/* 计算交点 */
ex = vdiff(*result2, *result1); // result1-result2向量
h = vnorm(ex); // 向量长度
ex = vdiv(ex, h); // 单位化
t2 = vmul(ex, mu * h);
t2 = vsum(*result1, t2);
*best_solution = t2; // 最佳解决方案
}
else
{
/* 如果mu1和mu2都在0到1之间 */
/* result1-result2线段与球4有两个交点 */
mu = (mu1 + mu2) / 2.0;
/* 计算交点 */
ex = vdiff(*result2, *result1); // result1-result2向量
h = vnorm(ex); // 向量长度
ex = vdiv(ex, h); // 单位化
t2 = vmul(ex, mu * h);
t2 = vsum(*result1, t2);
*best_solution = t2; // 最佳解决方案
}
}
return TRIL_4SPHERES; // 返回表示四个球面交点的结果
/******** 结束通过引入第四个球来找到一个解的计算 *********/
}
这段代码是一个用于三边测量的C语言函数，它的目的是利用一个点到其他几个已知点的距离来确定该点在三维空间中的位置。这个函数处理四个已知点（p1, p2, p3, p4）及这些点到未知点的距离（r1, r2, r3, r4）。主要目标是找出在所有四个已知点范围内的未知点的位置。
以下是对函数关键组成部分的逐项解释：
-
函数参数：
vec3d *result1, *result2, *best_solution：指向vec3d结构体的指针，用于存储计算出的位置。vec3d p1, p2, p3, p4：已知点的位置。double r1, r2, r3, r4：未知点到每个已知点的距离。double maxzero：一个小值，用于处理数值误差或判断值是否接近零。
-
局部变量：
- 像
ex, ey, ez这样的变量代表3D空间中的单位向量。 h, i, j, x, y, z, t等标量用于中间计算。
- 像
-
检查同心球体：
- 函数首先检查已知点（球体）是否有同心的（即具有相同中心）。如果有两个球体是同心的，函数会返回错误代码
ERR_TRIL_CONCENTRIC。
- 函数首先检查已知点（球体）是否有同心的（即具有相同中心）。如果有两个球体是同心的，函数会返回错误代码
-
计算交点：
- 函数的核心是进行数学计算，找到位于球体交点的点。这是通过向量运算和几何原理完成的。
-
处理特殊情况：
- 函数还处理了一些特殊情况，比如共线点（直线上的点）或者交点与球面相切。
-
使用第四个球体：
- 该函数使用第四个球体来细化解决方案。它检查由潜在解决方案（result1，result2）形成的线与第四个球体的交点。
-
选择最佳解决方案：
- 函数尝试基于与第四个球体的接近程度和交点的有效性来选择最佳解决方案。
-
返回结果：
- 函数设置
result1、result2和best_solution为计算出的位置。返回值指示计算的状态，如TRIL_3SPHERES、TRIL_4SPHERES或错误代码。
- 函数设置
-
错误处理：
- 函数包括对无效条件的检查，如负平方根或与第四个球体无交点，返回适当的错误代码。
总体而言，这个函数是三边测量的复杂实现，处理了各种边缘情况，并使用高级几何计算来找出3D空间中的位置。
kalman.c
/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "kalman.h"
#include "matrix.h"
/*==============================================================================
1.预估计
X(k|k-1) = F(k,k-1)*X(k-1|k-1) //控制量为0
2.计算预估计协方差矩阵
P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q(k)
Q(k) = U(k)×U(k)'
3.计算卡尔曼增益矩阵
Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R(k))
R(k) = N(k)×N(k)'
4.更新估计
X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
5.计算更新后估计协防差矩阵
P(k|k) =（I-Kg(k)*H）*P(k|k-1)
6. 更新最优值
F(k,k-1): 状态转移矩阵
X(k|k-1): 根据k-1时刻的最优值估计k时刻的值
X(k-1|k-1): k-1时刻的最优值
P(k|k-1): X(k|k-1)对应的covariance
P(k-1|k-1): X(k-1|k-1)对应的covariance
Q(k): 系统过程的covariance
R(k): 测量过程的协方差
H(k): 观测矩阵转移矩阵
Z(k): k时刻的测量值
基本思路: 首先根据上一次(如果是第一次则根据预赋值计算)的数据计算出本次的估计值,
同理,根据上一次的数据计算出本次估计值的协方差; 接着,由本次估计值的协
方差计算出卡尔曼增益; 最后,根据估测值和测量值计算当前最优值及其协方差
==============================================================================*/
//================================================//
//== 最优值方差结构体 ==//
//================================================//
typedef struct _tCovariance
{
float PNowOpt[LENGTH];
float PPreOpt[LENGTH];
}tCovariance;
static tOptimal tOpt;
static tCovariance tCov;
//float Z[LENGTH] = {4000}; // 测量值(每次测量的数据需要存入该数组)
static float I[LENGTH] = {1}; // 单位矩阵
static float X[LENGTH] = {9.8}; // 当前状态的预测值
static float P[LENGTH] = {0}; // 当前状态的预测值的协方差
static float K[LENGTH] = {0}; // 卡尔曼增益
static float Temp3[LENGTH] = {0}; // 辅助变量
//============================================================================//
//== 卡尔曼滤波需要配置的变量 ==//
//============================================================================//
static float F[LENGTH] = {1}; // 状态转移矩阵 即：坚信当前的状态与上一次状态的关系，如果坚信是一样的，则状态转移矩阵就为单位矩阵
static float Q[LENGTH] = {0.0001f};//0.0001f // 系统过程的协方差 协方差的定义：真实值与期望值之差的平方的期望值
static float R[LENGTH] = {2}; // 测量过程的协方差 协方差的定义：真实值与期望值之差的平方的期望值
//如果你需要滤波结果更依赖于观测量，那就调小R，增大Q；反之，调大R，调小Q，这样估计值就取决于系统。
//如果R大Q小，就是说，状态估计值比测量值要可靠，这时，所得出的结果就是更接近估计值；
//如果R小Q大，这时，计算出来的结果就会更接近测量值。
static float H[LENGTH] = {1}; // 观测矩阵转移矩阵 测量值与状态预测值之间的单位换算关系，即把预测值单位换算成测量值单位
static float Temp1[LENGTH] = {1}; // 辅助变量, 同时保存tOpt.XPreOpt[]的初始化值
static float Temp2[LENGTH] = {10000}; // 辅助变量, 同时保存tCov.PPreOpt[]的初始化值
void KalMan_PramInit(void)
{
unsigned char i;
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp1[i]; //零值初始化
}
for (i=0; i<LENGTH; i++)
{
tCov.PPreOpt[i] = Temp2[i]; //零值初始化
}
}
//============================================================================//
//== 卡尔曼滤波 ==//
//============================================================================//
//==入口参数: 当前时刻的测量值 ==//
//==出口参数: 当前时刻的最优值 ==//
//==返回值: 当前时刻的最优值 ==//
//============================================================================//
float KalMan_Update(double *Z)
{
u8 i;
MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系统的上一状态而预测现在状态; X(k|k-1) = F(k,k-1)*X(k-1|k-1)
MatrixCal(F, tCov.PPreOpt, Temp1, ORDER);
MatrixAdd(Temp1, Q, P, ORDER, ORDER); // 预测数据的协方差矩阵; P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q
MatrixCal(H, P, Temp1, ORDER);
MatrixAdd(Temp1, R, Temp1, ORDER, ORDER);
Gauss_Jordan(Temp1, ORDER);
MatrixTrans(H, Temp2, ORDER, ORDER);
MatrixMul(P, Temp2, Temp3, ORDER, ORDER, ORDER);
MatrixMul(Temp1, Temp3, K, ORDER, ORDER, ORDER); // 计算卡尔曼增益; Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
MatrixMul(H, X, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(Z, Temp1, Temp1, ORDER, ORDER);
MatrixMul(K, Temp1, Temp2, ORDER, ORDER, ORDER);
MatrixAdd(X, Temp2, tOpt.XNowOpt, ORDER, ORDER); // 根据估测值和测量值计算当前最优值; X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
MatrixMul(K, H, Temp1, ORDER, ORDER, ORDER);
MatrixMinus((double *)I, Temp1, Temp1, ORDER, ORDER);
MatrixMul(Temp1, P, tCov.PNowOpt, ORDER, ORDER, ORDER); // 计算更新后估计协防差矩阵; P(k|k) =（I-Kg(k)*H）*P(k|k-1)
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
return tOpt.XNowOpt[0];
}
这段代码是针对STM32F10x系列微控制器的卡尔曼滤波器的一个实现。卡尔曼滤波器是一种高效的递归滤波器，可以从一系列噪声测量中估计线性动态系统的状态。下面是这段代码的详细解释：
头文件
- stm32f10x.h: 这可能是STM32标准外设库的头文件，为STM32F10x系列微控制器硬件提供定义和函数。
- kalman.h: 这可能是卡尔曼滤波器定义和原型的头文件。
- matrix.h: 这可能是一个用于卡尔曼滤波器计算所需的矩阵操作的实用程序。
代码概览
-
注释: 顶部的注释用中文解释了卡尔曼滤波过程的步骤。包括预测、计算预测协方差矩阵、计算卡尔曼增益、更新估计以及更新估计协方差矩阵。
-
数据结构:
tCovariance: 一个结构体，用于保存当前和之前的协方差矩阵。tOptimal,tCov: 分别是最优值和协方差矩阵的实例。
-
静态变量: 声明了用于卡尔曼滤波计算的变量，如
I（单位矩阵）、X（状态估计）、P（估计协方差）、K（卡尔曼增益）等。 -
初始化函数 (
KalMan_PramInit): 初始化最优值和协方差矩阵到起始值。 -
卡尔曼更新函数 (
KalMan_Update):- 输入: 当前的测量值（
Z）。 - 输出: 更新后的最优值。
- 过程: 这是执行卡尔曼滤波算法的主要函数。它计算预测状态和协方差，计算卡尔曼增益，更新估计值，并重新计算协方差矩阵。
- 输入: 当前的测量值（
代码中的卡尔曼滤波步骤:
-
预测:
- 根据前一状态（
X(k-1|k-1)）使用状态转移矩阵（F）估计下一个状态（X(k|k-1)）。 - 计算预测协方差矩阵（
P(k|k-1)）。
- 根据前一状态（
-
更新:
- 计算卡尔曼增益（
Kg(k)），这决定了预测应该基于新测量值进行多少修正。 - 使用新的测量值（
Z(k)）更新估计值。 - 更新估计协方差矩阵以反映新估计值。
- 计算卡尔曼增益（
-
循环: 计算完成后，它会更新下一次迭代的前一状态和协方差矩阵。
特别说明:
- 代码使用自定义的矩阵操作（如
MatrixMul,MatrixCal,MatrixAdd等），这些可能在"matrix.h"或类似的实用程序文件中定义。 - 它针对特定的系统长度（由
LENGTH表示）并使用一维数组来表示矩阵，这意味着它可能是为具有单一状态变量的系统设计的，或者已经被线性化以使用一维数组来提高效率。 - 代码中直接没有显示错误处理、边界条件或特定硬件指令，但它们可能存在于实现的其他地方，或假设由系统性质处理。
总结
所提供的代码是针对STM32微控制器的卡尔曼滤波器的结构化和详细实现。它旨在估计并更新系统状态，随着时间的推移基于噪声测量，使用一系列矩阵操作和统计计算来提供系统当前状态的最佳估计。这种实现对于嵌入式系统来说是相当典型的，特别是在机器人、航空航天以及任何其他需要对噪声数据进行实时滤波的领域。
DWM1000初始化
void reset_DW1000(void)
void reset_DW1000(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// 启用用于DW1000复位的GPIO
GPIO_InitStructure.GPIO_Pin = DW1000_RSTn; // 指定复位引脚
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 设置为推挽输出模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // 设置GPIO速度为50MHz
GPIO_Init(DW1000_RSTn_GPIO, &GPIO_InitStructure); // 初始化GPIO
// 将RSTn引脚拉低，开始复位操作
GPIO_ResetBits(DW1000_RSTn_GPIO, DW1000_RSTn);
// 等待一段时间，确保复位信号被DW1000接收
deca_sleep(10);
// 将RSTn引脚拉高，结束复位操作
GPIO_SetBits(DW1000_RSTn_GPIO, DW1000_RSTn);
// 将RSTn引脚设置回三态模式，使其不干预DW1000工作
GPIO_InitStructure.GPIO_Pin = DW1000_RSTn;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; // 设置为模拟输入模式，相当于高阻态
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(DW1000_RSTn_GPIO, &GPIO_InitStructure);
// 等待一段时间，让复位操作稳定
deca_sleep(2);
}
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
dwt_initialise(DWT_LOADUCODE);
/* Configure DW1000. See NOTE 6 below. */
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 2 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
dwt_setinterrupt(DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
/* Set expected response's delay and timeout. See NOTE 1 and 5 below.
/* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
这段代码是为DW1000无线收发模块的初始化和配置流程。DW1000是一种用于精确测距的集成电路，常用于实现超宽带（UWB）通信。下面是这些函数和注释的详细解释：
reset_DW1000();
- 这个函数用来重置DW1000设备。注释中提到的“RSTn line”指的是连接到DW1000模块的复位引脚。驱动这个引脚为低电平一段时间，可以让模块进行硬件复位。
dwt_initialise(DWT_LOADUCODE);
- 这个函数初始化DW1000模块。
DWT_LOADUCODE参数指示模块在初始化过程中加载微代码（u-code）。微代码是一种在设备运行时用于优化性能的小段代码。
dwt_configure(&config);
- 这个函数用一个配置结构体来设置DW1000的参数。这个配置可能包含了如频率、脉冲重复频率、带宽、数据速率等多种参数。
dwt_setrxantennadelay(RX_ANT_DLY);
- 设置接收天线的延迟时间。这是必要的，因为从天线接收信号到模块处理该信号之间存在物理延迟。
dwt_settxantennadelay(TX_ANT_DLY);
- 设置发送天线的延迟时间。类似地，从模块发送信号到天线实际发射该信号也存在延迟。
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
- 这个函数设置了设备在发送完一个信号（比如轮询请求）后，应该等待多久才开始监听响应信号。这个延迟是以微秒为单位设置的。
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
- 这个函数设置了接收超时时间，即设备等待响应信号的最长时间。如果在这个时间内没有收到响应，设备可能会决定重发请求或者采取其他措施。
注释中提到的NOTE 1、NOTE 2和NOTE 5、NOTE 6很可能是文档中的脚注或者注释，用来给出关于这些函数调用的额外信息或者说明为什么要这样设置这些值。
总的来说，这段代码负责初始化DW1000模块，加载必要的微代码，配置模块的硬件参数，设置天线的延迟，以及定义在发送和接收数据时的时间参数。这些设置是为了确保无线信号的准确测量和有效通信。
note：所有以dwt_ 开头的函数均为DWM1000 官方标准API。
-
后面四个标准API 比较重要，设定不好距离测试不准是小，很可能无法测距。
-
先说前两个 setrxantennadelay/settxantennadelay 从字面意思就可以理解，这个是设定天线延时的。 在原理分析过程中明确到，DWM1000 可以精确知道无线信号何时到达，而无线信号通过天线接收，然后送给DWM1000模块，天线收到信号到送给DWM1000 这个是花费时间的，同样，当发送的时候也是，DWM1000 只知道自己将数据送出去，送给天线的时间，而天线收到信号到发射到空中也是需要时间的。 前者称为rxantennadelay 后者称为txantennadelay。而这两个值通常较大，而且基本认为是一样的。
这段话是对天线延迟设置在DW1000模块中的作用和原理的解释。DW1000是一款用于精确距离测量的无线收发模块，它能够精确知道无线信号何时被接收。在无线通信中，信号在硬件组件之间的传输并不是瞬时的，这就涉及到了天线延迟。
rxantennadelay（接收天线延迟）：这个参数设定了从天线接收无线信号开始，到信号被DW1000模块处理的这段时间。当无线信号到达接收天线时，它需要一定的时间通过天线电路并被DW1000模块读取。这个延迟必须在系统中得到补偿，以确保距离测量的准确性。
txantennadelay（发送天线延迟）：类似地，这个参数设定了DW1000模块发送信号到信号通过发送天线并发射到空中这段时间的延迟。DW1000模块知道信号何时被送出，但天线实际上需要一些时间来发射这个信号。
这两个延迟是影响无线通信中信号传输时间测量的重要因素。在进行距离测量时，信号的传输时间通常会很短，因此即使是微小的延迟也必须被准确地测量并补偿，以确保系统的准确性。rxantennadelay和txantennadelay通常设置为较大的值，并且在大多数情况下，这两个值被假设为相同，因为信号在接收和发送天线路径上的传播时间通常是对称的。
在DW1000模块中设置这些天线延迟值是为了使得无线信号的飞行时间测量可以减去这些内部传输时间，从而得到更精确的距离测量结果。
- Tround - Treplay - DeciveA txantennadelay - DeciveB rxantennadelay - DeciveB txantennadelay - DeciveA rxantennadelay 才是无线信号真正在空中传输时间。
这段话描述的是在两个设备（设备A和设备B）之间进行无线通信时，如何从测量的总往返时间（Tround）中提取出无线信号实际在空中传输的时间。总往返时间是指信号从设备A发送到设备B，并从设备B返回到设备A的总时间。为了得到信号在空中传输的真实时间，需要从总往返时间中减去所有与设备处理和天线延迟相关的时间。以下是每个术语的详细解释：
Tround - 总往返时间，从设备A发送信号到设备B，然后设备B响应信号返回到设备A的时间总和。
Treplay - 这是设备B收到信号后，到它响应该信号之前的处理时间。这段时间可能包括信号的解码、处理和准备回应的时间。
DeviceA txantennadelay - 设备A发送天线的延迟时间，是指DW1000模块发送信号到天线并且天线发射该信号到空中的时间。
DeviceB rxantennadelay - 设备B接收天线的延迟时间，是指天线接收到信号传递到DW1000模块处理的时间。
DeviceB txantennadelay - 设备B发送天线的延迟时间，是指DW1000模块发送响应信号到天线并且天线发射该信号到空中的时间。
DeviceA rxantennadelay - 设备A接收天线的延迟时间，是指天线接收到响应信号传递到DW1000模块处理的时间。
将所有这些内部处理和天线延迟的时间从总往返时间中减去之后，剩余的就是无线信号在两个设备之间空中实际传输的时间。这个时间是非常重要的，因为它可以用来计算信号传播的距离。通过知道信号在空中的传播速度（通常接近光速），可以用这个传输时间来计算出设备A和设备B之间的物理距离。这在需要高精度的距离测量，如室内定位、无人机控制和其他许多应用中是非常重要的。
-
后面两个是控制射频接收开启时间以及timeout的。 当DWM1000 发送一条信息后，如果明确知道收到信息的其它模块会回复一条信息，那么就需要在发送后打开接收器。当然可以选择立即打开接收器，但是根据实验数据大概估计会回复数据的时刻，可以延时一段时间在打开接收器，这个延时设定就setrxaftertxdelay。 接上面，打开接收器就在等数据，如果等不到就过段时间自动关闭接收器，不能一直无限期等下去啊，这个时间段就是setrxtimeout设定的。
实验过程有时忘记加setrxaftertxdelay 或者timeout 设置的时间太短，都可能导致无法收到信息而报错。
重要API
这张图片显示了一系列用于超宽带（UWB）通信的API函数，这些函数通常用于控制如Decawave DW1000这样的UWB芯片。这些API允许用户初始化设备、发送和接收数据以及处理状态标志。以下是对这些函数的详细解释：
初始化（Init）
int dwt_initialise(uint16 config): 这个函数用于初始化UWB设备，参数config用于指定配置选项。返回值通常是一个状态码，表示初始化成功或失败。接收（RX）
int dwt_rxenable(int delayed): 启用设备的接收模式。参数delayed指定是否延迟接收操作，这对于同步操作很重要。void dwt_setrxtimeout(uint16 time): 设置接收超时，即在放弃等待接收数据之前等待的时间长度。void dwt_readrxdata(uint8 *buffer, uint16 length, uint16 rxBufferOffset): 从UWB设备的接收缓冲区读取数据。buffer是存储读取数据的数组，length是要读取的数据量，rxBufferOffset是缓冲区中的偏移量。发送（TX）
int dwt_writetxdata(uint16 txFrameLength, uint8 *txFrameBytes, uint16 txBufferOffset): 将数据写入发送缓冲区。txFrameLength是数据帧的长度，txFrameBytes是包含数据的数组，txBufferOffset是缓冲区的偏移量。int dwt_writetxfctrl(uint16 txFrameLength, uint16 txBufferOffset): 设置发送帧控制参数，包括帧长度和缓冲区偏移量。int dwt_starttx(uint8 mode): 开始发送操作。参数mode定义了发送的模式，如立即发送或延迟发送。状态标志（Flag）
SYS_STATUS_RXFCG: 这是一个状态标志，表明已成功接收到数据帧。SYS_STATUS_ALL_RX_ERR: 这是一个状态标志，表明在接收过程中出现了错误。SYS_STATUS_TXFRS: 这是一个状态标志，表明数据帧已成功发送。在UWB应用程序中，您会使用这些函数来控制通信流程，如初始化设备、等待接收传入的通信、处理接收到的数据、发送响应等。通过正确的使用这些API，可以实现两个或多个UWB设备之间的精确距离测量和定位。
这张图展示了两个与时间戳相关的API函数，这些函数通常用于UWB（超宽带）通信中获取发送（TX）和接收（RX）消息的确切时间点。这些时间戳对于计算信号的飞行时间（Time of Flight, ToF）至关重要，进而用于测距和定位。下面是对这些函数的详细解释：
get_tx_timestamp_u64(): 这个函数用于获取最近一次发送消息的时间戳。返回的时间戳是64位的，这意味着它能提供非常高的时间分辨率，这对于UWB技术中的精确时间测量是必需的。时间戳以单位为基础（如微秒或更小的单位），依赖于硬件的时钟频率。
get_rx_timestamp_u64(): 类似地，这个函数用于获取最近一次接收消息的时间戳。它同样返回一个64位的时间戳，提供高精度的接收时间点。在UWB的DS TWR测距过程中，首先由一个设备发送一个信号（Poll），然后另一个设备接收该信号并响应（Response）。测距的精度依赖于能够精确地测量这些事件发生的时间。
get_tx_timestamp_u64()和get_rx_timestamp_u64()这两个函数提供了必要的高精度时间戳，使得系统能够计算出信号的往返时间。将这个时间乘以电磁波传播速度（在空气中接近光速），就可以得到信号传播的距离。这种方法是实现室内导航、资产跟踪和其他需要精密定位的系统的基础。
两种发送模式
这张图片描述了两种超宽带（UWB）通信中开始发送（TX）数据的模式。在UWB通信中，特别是在时间敏感的应用如精确测距和定位中，发送数据的时机非常关键。以下是对这两种模式的解释：
-
立即发送（Immediate Transmission）：
dwt_starttx(DWT_START_TX_IMMEDIATE);- 这条命令指示UWB设备立即开始发送数据。当函数被调用时，如果传输队列是空的，数据会立刻发送。
-
定时发送（Delayed Transmission）：
dwt_starttx(DWT_START_TX_DELAYED);- 这条命令配置UWB设备在设定的将来时间点发送数据。这通常用于同步传输，例如在时间同步的网络中保持通信的准确时序。
这两种模式的选择取决于应用程序的具体需求：
- 立即发送模式在不需要严格时间同步的情况下使用，例如，当数据需要尽快发送时。
- 定时发送模式在需要精确控制数据发送时间的应用中使用，这对于避免信道冲突和实现低延迟通信网络至关重要。
在定位和测距系统中，使用定时发送模式可以确保消息交换的时间间隔精确无误，这对于计算飞行时间（ToF）和随后的距离测量非常重要。
测距原理
[DWM1000 多个基站定位讨论 –蓝点无限]
[DWM1000 测距原理简单分析 之 SS-TWR代码分析2 – 蓝点无限]
[DWM1000 测距原理简单分析 之 SS-TWR代码分析1 – 蓝点无限]
这张图是一个时间同步的通信过程的示意图，用于说明设备A和设备B之间的三次消息交换。这种方法通常被称为双向时间传输（Dual-sided Two-Way Ranging, DS-TWR），用于计算两个设备之间的距离。
在图中，我们可以看到：
- 设备A（顶部）开始通过发送一个“POLL”消息（TX）到设备B（底部）来启动通信。
- 设备B接收到POLL消息（RX），并在一个预定的时间间隔后，发送一个应答消息（ACK）回到设备A。
- 设备A收到ACK消息后，经过一段时间，发送一个“FINAL”消息给设备B。
每个消息的发送（TX）和接收（RX）都被记录下来，这样可以计算出消息在两个设备之间传递所需的时间。图中的各个T_round和T_reply标记代表了各个阶段的往返时间。
这些时间戳可以用于计算从设备A到设备B以及回来的总时间。根据这些计算结果，可以估算出设备之间的距离，因为信号的传播速度（通常是光速）是已知的。
注意，这个图示可能是某种具体协议的简化表示，实际应用中可能需要考虑更多的因素，如信号传播延迟、设备内部处理时间等。
误差产生
高精度晶振2ppm，而质量差一点的40ppm，相差20倍。（摘自百度：ppm是个相对偏差，1ppm表示百万分之一，跟百分比一个道理。晶振频率一般以MHz（10的6次方）为单位，所以，标称频率10MHz的晶振，频率偏差10Hz就刚好是1ppm）
可以计算出2ppm 到 40 ppm 相差3.8ns，而光1ns传播30cm，也就是误差达到1m以上。所以晶振质量至关重要！
再来，可以纵向比较，前面说过 极度降低Treply 的时间
假如同样是2ppm 的晶振，Treply 增加引入的误差也是线性的，如果Treply = 5ms，引入的误差也超过1m
您上传的图片是一张表格，名为“表63：SS-TWR飞行时间估计中典型的时钟引起的误差”。SS-TWR代表单边双向测距，这是一种用于估计信号飞行时间的方法，通常用于雷达、声纳或无线通信系统中测量距离。
这张表格提供了由于时钟不准确而在飞行时间测量中引入的误差数据。以下是其内容的详细解释：
-
左侧列标为"T_reply"，列出了从100微秒（µs）到5毫秒（ms）的各种回复时间。这些回复时间代表测量的时间间隔。
-
顶部行列出了时钟误差，单位是百万分之几（ppm），范围从2 ppm到40 ppm。时钟的准确性通常用ppm来描述，它表示每百万时间单位的误差数量。
-
表格内的单元格给出了对应于T_reply（回复时间）和时钟误差组合的引起的误差，单位是纳秒（ns）。例如，如果时钟误差是2 ppm且T_reply是200微秒，那么引入的误差是0.2纳秒。
表中突出显示的单元格（对于T_reply是200微秒和500微秒，对于时钟误差是2 ppm到40 ppm）可能指向特别关注的数据点，或者是用户的常见场景。
这种表格对于理解时钟对于给定响应时间需要多精确以达到特定的飞行时间测量精度非常重要。例如，在设计和评估依赖精确时序测量的系统时，如电信网络同步、导航系统（如GPS）或任何需要通过测量信号往返目标的时间来精确确定距离的应用。
双向定位
从图中可以看到，两个阶段分别是发现阶段（Discovery Phase）和轮训阶段（Ranging Phase）。
其中Discovery Phase主要涉及到两种数据，BLink 和 Ranging Init，主要完成的任务是TAG 和ANCHOR配对。简单来说，TAG发送BLINK找ANCHOR，ANCHOR收到TAG的BLINK信息后分配短地址等信息，用Ranging Init信号。 在定位过程中，Discovery Phase只需要执行一次。
Ranging Phase，可以叫做定位阶段，主要是TAG发送数据给ANCHOR Poll（分别记录发送和接收时间），ANCHOR应答Response，以及TAG发送Final，具体包含的内容以及可以可以参考官方文档。
这个图表展示了两个阶段的无线通信过程，通常用于定位系统中的标签（Tag）和锚点（Anchor）之间的发现（Discovery）和测距（Ranging）。
发现阶段（Discovery Phase）：
-
未配对锚点（Unpaired Anchor）：处于监听模式，寻找标签的闪烁消息（Blink）。
- 锚点在等待收到任何标签发出的信号，以便开始配对过程。
-
未配对标签（Unpaired Tag）：发送周期性的闪烁消息（Blink），监听响应并休眠。
- 标签定期发送一个简短的信号来宣告其存在，并在发送后进入低功耗的休眠状态，以节省电能。
-
当锚点决定与某个标签进行测距时，它会发送测距初始化消息（Ranging Init）。
-
标签收到测距初始化消息后，会与锚点配对准备进行距离测量。
测距阶段（Ranging Phase）：
-
轮询（Poll）：测距过程开始时，锚点发送一个轮询消息给标签。
-
响应（Response）：标签接收到轮询消息后，回复一个响应消息给锚点。
-
最终消息（Final）：锚点在收到响应后，计算距离并发送一个最终消息给标签，该消息可能包含距离计算结果。
-
锚点在发送最终消息后，会监听下一个轮询消息，这样的过程可能会持续进行，以不断更新距离信息。
-
标签在发送轮询消息之间可能会休眠，以降低能耗。
整个过程是为了使锚点和标签之间建立通信并测量他们之间的距离。这种机制通常用于实时定位系统（Real-Time Locating Systems, RTLS）或室内导航系统，其中需要精确测量和跟踪标签的位置。
这张图表是关于无线定位系统中消息格式的编码结构，特别是在进行距离测量（ranging）时所使用的不同消息类型。图表描述了四种消息：轮询消息（Poll Message）、响应消息（Response Message）、最终消息（Final Message）、以及测距初始化消息（Ranging Init Message）。每种消息都有一定的格式，包含了不同的字段，这些字段对于执行时间同步和距离测量至关重要。下面是对每种消息及其字段的详细解释：
-
轮询消息（Poll Message）：
- 功能代码（Function code）：0x21，用于标识这是一个轮询消息。
-
响应消息（Response Message）：
- 功能代码（Function code）：0x10，标识这是一个响应消息。
- 活动参数（Activity Parameter）：0x02，可能表示消息的特定活动或状态。
- 活动参数（Activity Parameter）：0x0000，可能表示对特定活动或状态的附加信息。
- 先前的ToF（Previous ToF）：在响应消息中没有使用。
-
最终消息（Final Message）：
- 功能代码（Function code）：0x29，标识这是一个最终消息。
- 轮询消息TX时间戳（Poll Message TX Time-Stamp）：不使用。
- 响应消息RX时间戳（Response Message RX Time-Stamp）：不使用。
- 嵌入式预测最终TX时间戳（Embedded Predicted Final TX Time-Stamp）：不使用。
-
测距初始化消息（Ranging Init Message）：
- 功能代码（Function code）：0x20，标识这是一个测距初始化消息。
- 标签16位地址（Tag 16-bit Address）：用于标识标签的唯一地址。
- 响应延迟锚（Response Delay Anc）：不使用。
- 响应延迟标签（Response Delay Tag）：不使用。
图中显示的每个消息类型的框架结构都有一系列的字节，这些字节代表了消息的不同部分，包括功能代码和可能的时间戳或地址信息。这些消息会在定位系统的锚点和标签之间交换，以便进行时间同步和测距操作。例如，在两者之间测量距离时，会使用这些消息来记录发送和接收各个消息的时间点，从而计算出传播时间，进一步推算出距离。
如何使用三边定位代码
【新提醒】如何在标签上用三遍定位显示XY - 问答社区 51uwb.cn
-
int GetLocation(vec3d best_solution, int use4thAnchor, vec3d anchorArray, int *distanceArray)
-
这个函数参数说明，
- 第一个参数best_solution，其实是用来接收计算结果的。（指针）
- 第二个参数use4thAnchor，是否有4个基站，如果2D定位，3基站，将其设置为0，否则设置为1
- 第三个参数anchorArray，传递基站的坐标，有x y z
- 第四个参数distanceArray， 标签与基站对应的距离
返回值，为是否定位成功。
- step1 定义需要传递的参数类型。
vec3d AnchorList[ANCHOR_MAX_NUM];
vec3d tag_best_solution;
int Anthordistance[ANCHOR_MAX_NUM];
- step2 配置基站的坐标，需要根据基站放置的坐标实际配置
AnchorList[0].x =0.12;
AnchorList[0].y =0.34;
AnchorList[0].z =0;
AnchorList[1].x =0.25;
AnchorList[1].y =0;
AnchorList[1].z =0;
AnchorList[2].x =0;
AnchorList[2].y =0;
AnchorList[2].z =0;
- step3 传递测距信息到Anthordistance
while(Anchor_Index < ANCHOR_MAX_NUM)
{
if(Anthordistance_count[Anchor_Index] > 0 )
{
Anthordistance[Anchor_Index] =filter((int)(Anthordistance[Anchor_Index]/Anthordistance_count[Anchor_Index]),Anchor_Index);
}
Anchor_Index++;
}
- step4 调用GetLocation，这一步代码中没有实现。建议在distance_mange 函数中实现，因为这里已经将Anthordistance 赋值为最新的距离。
- step5 根据返回值，将tag_best_solution 中的x 和 y 打印出来就是定位解算的标签坐标。
要在 main.c 中使用三边定位代码并提高定位准确率，你需要做以下事情：
-
整合代码：首先，确保你有所有必要的三边定位函数和数据结构定义在你的
main.c或相关的头文件中。这包括vec3d结构、所有三边定位相关的函数等。 -
获取锚点和距离：你需要从你的系统中获取到三个或四个锚点的坐标（可能是固定的，并已知的），以及目标到每个锚点的距离。这些距离可以通过UWB或其他测距技术获得。
-
数据准备：在调用三边定位函数之前，准备好所有锚点的
vec3d数据结构，并将测距数据填充到距离数组中。 -
调用定位函数：使用准备好的数据调用
GetLocation或GetLocation2函数，这将返回最佳估计的位置。 -
优化和校准：为了提高定位准确性，你可能需要校准你的系统。校准可以包括：
- 确保你的锚点坐标是准确的。
- 对距离测量进行偏差校正。
- 采用滤波算法（例如卡尔曼滤波器）来平滑数据。
-
错误处理和重试逻辑：三边定位函数可能返回错误或多个解决方案。你的代码需要能够处理这些情况，可能是通过选择最接近第四个锚点的解决方案，或者在有多个可能的位置时选择最有意义的一个。
-
实时更新和反馈：在动态系统中，实时更新位置并根据反馈调整是提高准确率的关键。你可能需要实施一个反馈系统，根据实际的运动或外部输入来不断优化位置估计。
下面是一个简化的代码示例，说明如何在main.c中实现上述步骤：
#include "trilateration.h"
#include <stdio.h>
// 假设的锚点坐标
vec3d anchors[3] = {
{0.0, 0.0, 0.0}, // 锚点A的坐标
{5.0, 0.0, 0.0}, // 锚点B的坐标
{2.5, 5.0, 0.0} // 锚点C的坐标
};
// 测距数据
int distances[3] = {2500, 4000, 3500}; // 单位为毫米
int main() {
vec3d best_solution;
int use4thAnchor = 0; // 假设我们没有第四个锚点
int status = GetLocation(&best_solution, use4thAnchor, anchors, distances);
if (status == -1) {
printf("定位失败\n");
} else {
// 打印最佳位置估计
printf("定位成功: x = %f, y = %f, z = %f\n",
best_solution.x, best_solution.y, best_solution.z);
}
// 可以在这里加入优化和校准的逻辑
// ...
return 0;
}
这段代码是一个简化的框架，它假设你已经有了一个正确配置的三边定位系统，并且你有可靠的距离测量。在实际应用中，你需要根据你的具体硬件和软件环境调整这些步骤。
如何使用卡尔曼滤波代码
将卡尔曼滤波应用到main.c文件中，主要涉及以下几个步骤：
1. 引入卡尔曼滤波算法：
- 首先确保你有一个可用的卡尔曼滤波器的实现，通常是
.h和.c文件，例如前面提到的kalman.h和相应的实现文件。 - 在
main.c的开始部分包含卡尔曼滤波的头文件：#include "kalman.h"
2. 初始化卡尔曼滤波器：
- 在主程序或适当初始化部分调用卡尔曼滤波初始化函数，设置初始状态、误差协方差、过程噪声等。
- 例如，如果你有一个初始化函数
Kalman_Init(), 则在main函数中调用它。
// 定义卡尔曼滤波器实例
KalmanFilter_t kalmanFilter;
// 初始化卡尔曼滤波器（示例参数，具体需要根据实际情况调整）
void init_kalman_filter() {
// 假设一些初始值和协方差
float initial_estimate = 0.0f;
float initial_error_cov = 1.0f;
Kalman_Init(&kalmanFilter, initial_estimate, initial_error_cov);
}
3. 在数据接收处应用卡尔曼滤波：
- 在
main.c中，找到处理UWB消息和计算距离的部分。 - 在接收到新的距离测量或其他需要滤波的数据后，将这些数据作为输入传递给卡尔曼滤波器。
- 例如，在接收到最终距离消息后，使用卡尔曼滤波器更新估计。
// 在主循环中或数据接收回调函数中
while (1) {
// 假设这是从UWB模块接收到的新的测量距离
double measured_distance = get_new_uwb_distance();
// 使用测量值更新卡尔曼滤波器
double estimated_distance = Kalman_Update(&kalmanFilter, measured_distance);
// 现在可以使用平滑和估计的距离值了
// 例如，显示在LCD上或进行其他计算
}
4. 更新和获取卡尔曼滤波器的状态：
- 每当有新的测量值时，调用卡尔曼滤波的更新函数，如
Kalman_Update()。 - 使用卡尔曼滤波的输出更新系统状态或变量。
示例代码片段：
#include "stm32f10x.h"
#include "kalman.h"
// 全局变量定义卡尔曼滤波器实例
KalmanFilter_t kalmanFilter;
// 初始化卡尔曼滤波器
void init_kalman_filter() {
float initial_estimate = 0.0f;
float initial_error_cov = 1.0f;
Kalman_Init(&kalmanFilter, initial_estimate, initial_error_cov);
}
// 假设的获取新UWB距离的函数
double get_new_uwb_distance() {
// 这里应该是与UWB硬件通信的代码
// 返回测量的距离
return 2.5; // 假设的测量值
}
int main(void) {
// 硬件初始化代码...
// 初始化卡尔曼滤波器
init_kalman_filter();
while(1) {
// 获取新的UWB测量距离
double measured_distance = get_new_uwb_distance();
// 更新卡尔曼滤波器
double estimated_distance = Kalman_Update(&kalmanFilter, measured_distance);
// 使用estimated_distance进行其他操作，例如更新显示或计算
// ...
// 硬件或调度的延迟
// ...
}
}
// 硬件特定或其他函数...
5. 调试和测试：
- 在将卡尔曼滤波器集成到你的主程序中后，进行彻底的测试，确保它按预期工作，没有引入任何意外的行为或错误。
- 观察卡尔曼滤波器输出是否平滑了输入数据，同时保持对实际变化的敏感度。
注意：
- 卡尔曼滤波的实现和初始化高度依赖于具体的应用场景，包括系统模型、噪声特性等，因此需要根据实际情况调整参数和模型。
- 一定要理解卡尔曼滤波的基本原理和数学背景，这对于调试和优化滤波效果至关重要。
通过以上步骤，你可以将卡尔曼滤波器算法应用到main.c中，用于对接收到的测距数据或其他传感器数据进行平滑处理，以提高系统性能和准确性。
注意事项：
- 这个示例假设你有一个基本的卡尔曼滤波器实现和对应的初始化及更新函数。实际的函数名和参数可能需要根据你的实际卡尔曼滤波器代码进行调整。
- 确保卡尔曼滤波器的初始值、协方差和其他参数适合你的具体场景。
get_new_uwb_distance()函数需要你根据实际的UWB模块和硬件来实现，这里只提供了一个示例返回值。
三边定位系统，并且你有可靠的距离测量。在实际应用中，你需要根据你的具体硬件和软件环境调整这些步骤。
如何使用卡尔曼滤波代码
将卡尔曼滤波应用到main.c文件中，主要涉及以下几个步骤：
1. 引入卡尔曼滤波算法：
- 首先确保你有一个可用的卡尔曼滤波器的实现，通常是
.h和.c文件，例如前面提到的kalman.h和相应的实现文件。 - 在
main.c的开始部分包含卡尔曼滤波的头文件：#include "kalman.h"
2. 初始化卡尔曼滤波器：
- 在主程序或适当初始化部分调用卡尔曼滤波初始化函数，设置初始状态、误差协方差、过程噪声等。
- 例如，如果你有一个初始化函数
Kalman_Init(), 则在main函数中调用它。
// 定义卡尔曼滤波器实例
KalmanFilter_t kalmanFilter;
// 初始化卡尔曼滤波器（示例参数，具体需要根据实际情况调整）
void init_kalman_filter() {
// 假设一些初始值和协方差
float initial_estimate = 0.0f;
float initial_error_cov = 1.0f;
Kalman_Init(&kalmanFilter, initial_estimate, initial_error_cov);
}
3. 在数据接收处应用卡尔曼滤波：
- 在
main.c中，找到处理UWB消息和计算距离的部分。 - 在接收到新的距离测量或其他需要滤波的数据后，将这些数据作为输入传递给卡尔曼滤波器。
- 例如，在接收到最终距离消息后，使用卡尔曼滤波器更新估计。
// 在主循环中或数据接收回调函数中
while (1) {
// 假设这是从UWB模块接收到的新的测量距离
double measured_distance = get_new_uwb_distance();
// 使用测量值更新卡尔曼滤波器
double estimated_distance = Kalman_Update(&kalmanFilter, measured_distance);
// 现在可以使用平滑和估计的距离值了
// 例如，显示在LCD上或进行其他计算
}
4. 更新和获取卡尔曼滤波器的状态：
- 每当有新的测量值时，调用卡尔曼滤波的更新函数，如
Kalman_Update()。 - 使用卡尔曼滤波的输出更新系统状态或变量。
示例代码片段：
#include "stm32f10x.h"
#include "kalman.h"
// 全局变量定义卡尔曼滤波器实例
KalmanFilter_t kalmanFilter;
// 初始化卡尔曼滤波器
void init_kalman_filter() {
float initial_estimate = 0.0f;
float initial_error_cov = 1.0f;
Kalman_Init(&kalmanFilter, initial_estimate, initial_error_cov);
}
// 假设的获取新UWB距离的函数
double get_new_uwb_distance() {
// 这里应该是与UWB硬件通信的代码
// 返回测量的距离
return 2.5; // 假设的测量值
}
int main(void) {
// 硬件初始化代码...
// 初始化卡尔曼滤波器
init_kalman_filter();
while(1) {
// 获取新的UWB测量距离
double measured_distance = get_new_uwb_distance();
// 更新卡尔曼滤波器
double estimated_distance = Kalman_Update(&kalmanFilter, measured_distance);
// 使用estimated_distance进行其他操作，例如更新显示或计算
// ...
// 硬件或调度的延迟
// ...
}
}
// 硬件特定或其他函数...
5. 调试和测试：
- 在将卡尔曼滤波器集成到你的主程序中后，进行彻底的测试，确保它按预期工作，没有引入任何意外的行为或错误。
- 观察卡尔曼滤波器输出是否平滑了输入数据，同时保持对实际变化的敏感度。
注意：
- 卡尔曼滤波的实现和初始化高度依赖于具体的应用场景，包括系统模型、噪声特性等，因此需要根据实际情况调整参数和模型。
- 一定要理解卡尔曼滤波的基本原理和数学背景，这对于调试和优化滤波效果至关重要。
通过以上步骤，你可以将卡尔曼滤波器算法应用到main.c中，用于对接收到的测距数据或其他传感器数据进行平滑处理，以提高系统性能和准确性。
注意事项：
- 这个示例假设你有一个基本的卡尔曼滤波器实现和对应的初始化及更新函数。实际的函数名和参数可能需要根据你的实际卡尔曼滤波器代码进行调整。
- 确保卡尔曼滤波器的初始值、协方差和其他参数适合你的具体场景。
get_new_uwb_distance()函数需要你根据实际的UWB模块和硬件来实现，这里只提供了一个示例返回值。- 适当地调试和测试整合后的系统，确保卡尔曼滤波器正确地平滑了数据，并且系统按预期工作。
扫一扫
1672
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
