为物联网应用计算True North(译文)
By robot-v1.0
本文链接 https://www.kyfws.com/pi/calculating-true-north-for-iot-applications-zh/
版权声明 本博客所有文章除特别声明外,均采用 BY-NC-SA 许可协议。转载请注明出处!
- 20 分钟阅读 - 9588 个词 阅读量 0为物联网应用计算True North(译文)
原文地址:https://www.codeproject.com/Articles/1112064/Calculating-True-North-for-IoT-Applications
原文作者:CoderGirl42
译文由本站 robot-v1.0 翻译
前言
Finding true North using GPS and a compass with Raspberry Pi.
使用GPS和带有Raspberry Pi的指南针找到真正的北方.
介绍(Introduction)
查找您的地理位置和方向在机器人技术和嵌入式系统中很有用.使用GPS和指南针传感器,可以轻松确定相对于磁北的位置和方向.但是,如果您想知道与True North有关的方向怎么办?(Finding your geographic location and direction is useful in robotics and embedded systems. Using GPS and a Compass Sensor it’s easy to determine your location and your direction in relation to Magnetic North. But what if you want to know you direction in relation to True North?) 确定从磁北到真北的偏移量(也称为磁偏角)并不难.磁偏角可以很容易地在网上找到(It’s not difficult to determine your offset from Magnetic North to True North, also known as Magnetic Declination. Magnetic Declination can be easily found on the web through a) NOAA网站(NOAA website) .但是我想要一个可以通过代码在嵌入式设备中使用的实时模型,然后输入世界电磁模型(WMM)!在Linux中将GPS与WMM配合使用可以轻松确定磁偏角,然后可以使用该磁偏角从Compass偏移磁轴承.我将指导您完成确定磁偏角的步骤,并使用它来调整您的指南针轴承.(. But I wanted a real time model that I could use in my embedded device through code, enter the World Magnetic Model (WMM)! Using GPS with the WMM in Linux can easily determine Magnetic Declination that you can then use to offset the Magnetic Bearing from your Compass. I will take you through the steps to determine Magnetic Declination and use it to adjust your compass bearing.)
背景(Background)
我是乐高制造商,也是乐高原型挑战赛的获胜者.我一直在研究由乐高和Raspberry Pi组件制成的寻星相机机器人.为了确定恒星坐标,我首先需要以合理的准确度知道我的机器人的地理坐标以及相对于真实北方的机器人所面向的方向.设置GPS和指南针非常简单,安装和访问World Magnetic Model也非常简单.(I’m a LEGO maker and a winner in a LEGO prototype challenege. I have been working on a star finding camera robot made out of LEGO and Raspberry Pi components. To determine stellar coordinates I first need to know my robot’s geographic coordinates and which direction it’s facing in relation to true North with a reasonable degree of accuracy. Setting up the GPS and Compass was fairly straightforward and installing and accessing the World Magnetic Model was also fairly simple.) 这是我的第一篇CodeProject文章,我的机器人仍在开发中,但已经取得了长足的发展.我想分享一些我已经在社区中完成的工作.我希望你喜欢它!(This is my first CodeProject article and my robot is still a work in progress but has made it a long way. I wanted to share some of the work I’ve already done with the community. I hope you like it!)
您可以在这些链接中找到我的机器人和其他乐高设备的更多图像.(You can find more images of my robot and my other LEGO devices at these links.) Starbot(乐高相机)(Starbot (Lego Camera)) 乐高数码相框(Lego Digital Picture Frame) 傻瓜相机(Point & Shoot IR Camera) 乐高电脑(Lego Computer)
使用的组件(Components Used)
您需要设置以下设备,下面的链接中包含设置说明.我不会详细介绍这些主题,因此您需要对Raspberry Pi的设置以及Linux中的Digital和I2C传感器有所了解.(You’ll need to setup the following devices, instructions for setup are included in the links below. I will not cover these topics in detail so you’ll need to be somewhat familiar with Raspberry Pi setup and Digital and I2C sensors in linux.) 1个(1) Raspberry Pi 2 B型(Raspberry Pi 2 Model B) 跑步(running) 树莓派(Raspbian) .(.) 1个(1) Adafruit Ultimate GPS突破(Adafruit Ultimate GPS Breakout) ((() 设定说明(Setup Instructions) )()) 1个(1) HMC5883L三轴磁力仪(HMC5883L Triple Axis Magnetometer) ((() 设定说明(Setup Instructions) )())
接线图(Wiring Diagram)
所需的图书馆(Required Libraries)
您将需要在Raspberry Pi上安装一些库,这些库将使您可以通过代码访问GPS和电磁模型.指南针不需要其他库,但确实使用i2c-dev.h.(You’ll need a few libraries installed on your Raspberry Pi, these will give you access to your GPS and Magnetic Models through code. The compass does not require additional libriaries but it does use i2c-dev.h.) 全球定位系统(GPSd) ((() 设定说明(Setup Instructions) )(使用libgpsmm.h访问GPS数据)() (Access to GPS data using libgpsmm.h)) 地理图书馆(GeographicLib) ((() 设置说明(Setup Instructions) )(访问WMM数据和其他与地理相关的算法)() (Access to WMM data and additional Geographic related algorithms))
读取GPS坐标(Reading GPS Coordinates)
第一步是从设备读取GPS坐标.使用GPSd和libgpsmm.h可以非常轻松地访问GPS数据.我们将创建一个类来管理此接口,并为我们提供从设备流式传输的值. GPS使用串行连接,并且libgps将为您解析返回的句子.我们可以使用返回的原始坐标值,也可以将它们分为度,分和秒之类的组成部分.(The first step is to read the GPS coordinates from your device. Using GPSd and libgpsmm.h make it pretty simple to access your GPS data. We’re going to make a class to manage this interface and supply us with values streamed from the device. The GPS uses a serial connection and the sentences returned are parsed for you by libgps. We can either use the raw coordinate values returned or they can be broken into components like degrees, minutes and seconds.) GPS类(The GPS Class) 我创建了一个简单的包装器类,以管理与GPS库的交互,并协调原始值和组件值之间的转换.(I created a simple wrapper class to manage interactions with the GPS library and do coordinate conversions between raw and component values.)
#ifndef __GPS_H_INCLUDED__
#define __GPS_H_INCLUDED__
#include "libgpsmm.h" // GPS
class gps
{
private:
// Main entry into GPSd.
gpsmm *gps_rec;
// Private Helper functions for Converting Values.
int degrees(float coordinate);
int minutes(float coordinate);
float seconds(float coordinate);
public:
// Values for our Application to Consume.
int gps_fix;
float gps_lon;
float gps_lat;
float gps_alt;
int gps_sat;
int gps_use;
// Routines for managing interactions with GPSd.
bool start(); // Starts GPSd interface
bool update(); // Retrieves Coordinates from GPSd.
bool stop(); // Shuts down GPSd interface and releases resources.
// Conversion functions to return latitude components from raw GPS values.
int latitude_degrees();
int latitude_minutes();
float latitude_seconds();
// Conversion functions to return longitude components from raw GPS values.
int longitude_degrees();
int longitude_minutes();
float longitude_seconds();
};
#endif
初始化GPS流(Initializing GPS Stream) 现在,您的课程已设置完毕,您将需要初始化GPS流.只需几行代码,您就可以在此处修改一些选项,GPS设备地址和流格式,但是我使用了默认值.您可以在GPSd网站上进行其他研究,以确定如何使用参数.(Now that your class is setup you’ll need to initialize your GPS stream. It’s only a few lines of code, you’ll be able to modify a few options here, GPS device address and stream formatting, but I used the defaults. You can do additional research on the GPSd site to determine how to use the parameters.)
bool gps::start()
{
/* Initialize GPS */
gps_rec = new gpsmm("localhost", DEFAULT_GPSD_PORT);
/* Setup GPS Stream */
if (gps_rec->stream(WATCH_ENABLE | WATCH_JSON) == NULL)
{
return false;
}
return true;
}
读取GPS流(Reading GPS Stream)
现在,您的设备已设置完毕,您可以从流中读取值了.这也很简单,我创建了一个名为(Now that your device is setup you can read the values from the stream. This is also pretty simple to do, I created a function called) update()
然后在我的主控制循环的每一帧中调用一次.我们将使用一个名为(that is then called once per frame of my main control loop. We’ll use a data structure called) gps_data_t
由libgpsmm.h提供以存储原始值,然后将它们存储在我们的类级别变量中. gps_data_t返回很多值,您不妨参考(provided by libgpsmm.h to store the raw values and then store them in our class level variables. gps_data_t returns a lot of values, you may wish refer to the) 在线文档(online documentation) 看看是否还有其他值需要用于您的目的.(to see if there are additional values you may need to use for your purpose.)
bool gps::update()
{
/* Raw GPS Values structure */
struct gps_data_t* gps_data;
/* Wait until device is ready */
if (gps_rec->waiting(00050000))
{
/* See if we're reading data. */
if ((gps_data = gps_rec->read()) == NULL)
{
return false;
}
/* Check for a 2D or 3D fix. */
if (gps_data->status == STATUS_FIX &&
(gps_data->fix.mode == MODE_2D || gps_data->fix.mode == MODE_3D))
{
/* Fix Obtained, Set Values. */
gps_fix = gps_data->fix.mode;
gps_lon = gps_data->fix.longitude;
gps_lat = gps_data->fix.latitude;
gps_alt = gps_data->fix.altitude;
gps_sat = gps_data->satellites_visible;
gps_use = gps_data->satellites_used;
}
}
return true;
}
关闭GPS(Shutting Down GPS) 完成后,您需要关闭流并释放资源.(When you’re done you need to shutdown your stream and release your resources.)
bool gps::stop()
{
if(gps_rec->stream(WATCH_DISABLE) == NULL)
{
delete gps_rec;
return false;
}
delete gps_rec;
return true;
}
就这样,如您所见,从GPS设备设置和读取坐标非常简单.现在您已经有了原始坐标值,您可能需要解析十进制表示形式的组件值.原始值将"度",“分钟"和"秒"分量表示为单个十进制值.(That’s it, as you can see it’s pretty simple to setup and read coordinates from your GPS device. Now that you have raw coordinate values you may want to parse the component values from the decimal representation. The raw values represents the Degrees, Minutes, and Seconds components as a single decimal value.)
解析度(Parsing Degrees)
度数表示为原始值的整数部分,以便确定度数只截断浮点值.在这里,我只是投射到(Degress are represented as the whole number part of the raw value, in order to determine the degress just truncate the floating point values. Here I just cast to an) int
并返回绝对值.(and return the absolute value.)
/*
* degrees: converts decimal angle to degree component.
*
*/
int gps::degrees(float angle)
{
return abs((int)angle);
}
解析分钟(Parsing Minutes) 分钟是1度的六十分之一.从原始值中删除学位分量后,剩下的只有几分钟和几秒钟.要转换为分钟,您需要乘以60,然后截断浮点值以减少秒数.(Minutes are one sixtyth of one degree. Once you remove you degree component from your raw value you’re left with minutes and seconds. To convert to minutes you’ll need to multiply by 60 and then truncate the floating point values to drop the seconds.)
/*
* minutes: converts decimal angle to minutes component.
*
*/
int gps::minutes(float angle)
{
int deg = degrees(angle);
int min = (int)((fabs(angle) - deg) * 60.00);
return min;
}
解析秒(Parsing Seconds) 秒是一分钟的六十分之一.要获取分钟数,您需要执行两个步骤:首先,从原始坐标值中删除度,然后执行将其转换为分钟数的步骤.现在您有了分钟,可以减去整个值,然后再乘以60,这将使您剩下秒数部分.秒可以作为带分数的十进制值返回.(Seconds are one sixtyth of one minute. To get the minutes you’ll need to do two steps, First you remove the degrees from the raw coordinate value, next you perform the step to convert to minutes. Now that you have minutes you can deduct the whole value and again mulitply by 60, this will leave you with your seconds component. Seconds can be returned as a decimal value with fractional values.)
/*
* seconds: converts decimal angle to seconds component.
*
*/
float gps::seconds(float angle)
{
int deg = degrees(angle);
int min = minutes(angle);
float sec = (((fabs(angle) - deg) * 60.00) - (float)min) * 60.00;
return sec;
}
确定东方vs西方和北方vs南方(Determining East vs West and North vs South) gps座标的原始值中还有一个附加组件.此值确定经度的东/西和纬度的北/南.这被编码为原始GPS值的符号位.如果您的原始值是负数,则将指示西/南;如果是正值,则将指示东/北.(There is an additional component encoded into the raw value of your gps coordinates. This value determines East/West for Longitude and North/South for Latitude. This is encoded into the sign bit of the raw GPS value. if your original value is negative this will indicate West/South and if it is positive it will indicate East/North.)
gps_lat >= 0 ? 'N' : 'S'; // Determine North or South.
gps_lon >= 0 ? 'E' : 'W'; // Determine East or West.
示例数据(Example Data) 十进制纬度:-15.063888888888888(Decimal Latitude: -15.063888888888888) 换算的纬度:15°3'50"宽(Converted Latitude: 15° 3' 50” W) 十进制经度:45.33416666666667(Decimal Longitude: 45.33416666666667) 经度转换:45°20'3" N(Converted Longitude: 45° 20' 3" N)
世界磁模型和偏角(World Magnetic Model and Declination)
现在我们有了GPS坐标,下一步就是确定机器人的磁偏角.我们可以使用GeographicLib和World Magnetic Model做到这一点,但首先我们需要了解磁偏角的概念.(Now that we have our GPS coordinates the next step is to determine our robot’s Magnetic Declination. We can do this with GeographicLib and the World Magnetic Model but first we need to understand the concept of Magnetic Declination.) 偏角(Declination) 引用:https://en.wikipedia.org/wiki/Magnetic_declination(Quote: https://en.wikipedia.org/wiki/Magnetic_declination) **磁偏角(Magnetic declination)**要么(or)**变异(variation)**是磁北向(水平面北端的方向)与水平面之间的夹角(is the angle on the horizontal plane between magnetic north (the direction the north end of a) 罗盘(compass) 针尖,对应于方向(needle points, corresponding to the direction of the) 地球的(Earth’s) 磁力线(magnetic field lines) )和() and) 真正的北方(true north) (沿a的方向((the direction along a) 子午线(meridian) 走向地理(towards the geographic) 北极(North Pole) ).该角度根据地球表面的位置而变化,并随时间变化.(). This angle varies depending on position on the Earth’s surface, and changes over time.)
如您所见,磁偏角就是磁北和真北之间的偏移角.该值随位置和时间而变化.确保刷新此值很重要,因为它可能会在控制循环中的不同帧之间变化(这不太可能,但可能).还有一个类似的概念,称为倾斜度,它确定了垂直平面上磁场的角度,但是我们不需要此值来确定与北磁的偏移量.(As you can see Magnetic declination is simply the angle of offset between Magnetic North and True North. This value changes with position and time. It’s important to make sure to refresh this value since it could vary between frames in your control loop (It’s unlikely but possible). There is also a similar concept called inclination, this determines the angle of the magnetic field on the vertical plane but we don’t need this to determine our offset from Magnetic North.) 磁性模型(Magnetic Model) 为了确定磁偏角,我们需要能够理解机器人所在位置的磁力线.从描述中我们知道,磁偏角会随着位置和时间而变化.因此,如果不断变化,我们怎么知道我们的实际磁偏角呢?世界磁场模型就是答案,它是NOAA提供的一个预先计算的数据集,可以由GeographicLib使用.提供的数据集通常一次可以使用几年,并且需要定期更新以确保准确的信息.(To determine Magnetic Declination we need to be able to understand the magnetic field lines at our robot’s location. From the description we know that Magnetic Declination can change with position and time. So how would we know what our actual declination is if it’s constantly changing? The World Magnetic Model is the answer, it is a precomputed data set that is provided by NOAA and can be consumed by GeographicLib. The datasets provided are usually good for a few years at a time and need to be updated with regularity to ensure accurate information.)
您可以在以下位置找到更多信息和有用的工具(You can find more information and useful tools at the) NOAA网站(NOAA website) .(.) 使用GeographicLib(Using GeographicLib) 我围绕着GeographicLib库创建了一个简单的包装器,类似于GPS包装器.它具有我们要查询的值的属性,例如磁偏角和倾角.该代码实际上比GPS组件更易于使用,并且不需要start()或stop()步骤.(I created a simple wrapper around the GeographicLib library similar to the GPS wrapper. This has properties for the values we wish to query, such as declination and inclination. This code is actually easier to use then the GPS component and does not require a start() or stop() step.)
#ifndef __WMM_H__
#define __WMM_H__
#include <GeographicLib/MagneticModel.hpp> // Magnetic Model
using namespace GeographicLib;
class wmm
{
private:
double magnetic_declination;
double magnetic_inclination;
double field_strength;
public:
double declination();
double inclination();
double strength();
void update(float lat, float lon, float alt);
};
#endif // !__WMM_H__
偏向(Getting Declination)
我们需要使用GPS模块返回的纬度,经度和海拔高度(We’ll need to use the latitude, longitude and altitude returned by GPS Module for our) update(...)
功能.这将为我们提供3D位置,以确定地球产生的局部磁场.此外,我们还需要确定当前日期和时间,以返回当前时刻的准确电磁模型信息.(function. This will give us a 3D position to determine the local magnetic field produced by the Earth. Additionally we also need to determine the current date and time to return accurate Magnetic Model information for the current moment.)
void wmm::update(float lat, float lon, float alt)
{
/* intermediate values */
double Bx, By, Bz, H;
/* Determine current time. */
time_t t = time(NULL);
tm* timePtr = localtime(&t);
/*
* Magnetic Model component, Using emm2015 magnetic model
* (emm2015 includes magnetic interactions from Earth's crust)
*/
MagneticModel mag("emm2015");
/* Get intermediate values using current time and position */
mag(timePtr->tm_year + 1900, lat, lon, alt, Bx, By, Bz);
/* Convert intermediate values into field components and store in class members */
MagneticModel::FieldComponents(Bx, By, Bz, H,
field_strength, magnetic_declination, magnetic_inclination);
}
阅读指南针轴承(Reading Compass Bearings)
我们的指南针使用i2c接口,不需要i2c-dev之外的库.这使它的设置比我们的GPS稍微复杂一些,但幅度不大.返回的值应该是本地电磁轴承.这是我们需要补偿以确定True North方位的值.磁强计容易产生噪音,并且由于本地磁场的电磁干扰,磁强计可能无法随时间返回一致的值.这可能是由于传感器附近的其他金属或电子设备引起的,或者是由于我的跳线长度过长所致.您将需要确保指南针传感器尽可能与随机磁场隔离.(Our compass uses the i2c interface and does not require a library outside of i2c-dev. This makes it a little more complicated to setup than our GPS but not by much. The value returned should be the local Magnetic Bearing. This is the value we will need to offset to determine the bearing for True North. Magnetometers are prone to noise and may not return a consistent value over time due to magnetic interference from local magnetic fields. This can be due to other metals or electronics near your sensor or in my case the length of my jumper wires. You will need to ensure that your compass sensor is as isolated from random magnetic fields as possible.) 指南针类(Compass Class) 罗盘类非常简单,它与i2c交互,然后使用卡尔曼滤波器对值进行滤波,以实现更平滑的输出.(The compass class is pretty simple, it interacts with i2c and then filters the values using a Kalman filter for smoother output.)
#ifndef __COMPASS_H__
#define __COMPASS_H__
#include "kalman.h"
#define HMC5883L_I2C_ADDR 0x1E
class compass
{
private:
int i2c_fd;
/* i2c interaction functions */
bool select_i2c_device(int fd, int addr, char * name);
bool write_to_i2c(int fd, int reg, int val);
public:
/* Filtered Compass Bearing */
float bearing;
/* Routines for managing interactions with the compass sensor */
int start();
int update();
};
#endif
初始化指南针(Initializing the Compass)
要初始化指南针,我们首先需要选择我们的i2c设备,然后使用以下命令将一些数据写入总线(To initialize the compass we’ll first need to select our i2c device and then write some data to the bus using the following) select_i2c_device(...)
和(and) write_to_i2c(...)
功能.(functions.)
/* Select our i2c compass device */
bool compass::select_i2c_device(int fd, int addr, char * name)
{
if (ioctl(fd, I2C_SLAVE, addr) < 0) {
return false;
}
return true;
}
/* Write something to the i2c device */
bool compass::write_to_i2c(int fd, int reg, int val)
{
char buf[2];
buf[0]=reg;
buf[1]=val;
if (write(fd, buf, 2) != 2) {
return false;
}
return true;
}
现在在我们的(Now in our) start()
函数,我们可以调用以前的i2c函数来设置设备.(function we can call the previous i2c functions to setup our device.)
int compass::start()
{
init = true;
/* Initialize i2c compass */
if ((i2c_fd = open("/dev/i2c-1", O_RDWR)) < 0)
{
return 0;
}
/* initialise HMC5883L */
select_i2c_device(i2c_fd, HMC5883L_I2C_ADDR, "HMC5883L");
/* Set Initial register values*/
write_to_i2c(i2c_fd, 0x01, 32);
write_to_i2c(i2c_fd, 0x02, 0);
return 1;
}
阅读指南针轴承(Reading Compass Bearing)
现在应该可以阅读指南针了.与GPS和WMM一样,我们希望在位置或方向发生变化的情况下继续更新传感器值,这与我们将使用的其他组件类似(The compass should now be ready to read from. Like the GPS and WMM we want to keep updating our sensor value in case our position or orientation change, similar to the other components we will use the) update()
功能.(function.)
int compass::update()
{
float angle = 0;
unsigned char i2c_buf[16];
i2c_buf[0] = 0x03;
/* Send the register to read from */
if ((write(i2c_fd, i2c_buf, 1)) != 1)
{
return 0;
}
/* Read from the register values. */
if (read(i2c_fd, i2c_buf, 6) != 6)
{
return 0;
}
else
{
/* Convert raw byte values to shorts. */
short x = (i2c_buf[0] << 8) | i2c_buf[1];
short y = (i2c_buf[4] << 8) | i2c_buf[5];
/* Determine Compass Bearing Angle from raw x and y components. */
angle = atan2(y, x) * 180 / M_PI;
}
/* Set the filtered Bearing. */
bearing = angle;
return 1;
}
在这里,您可以看到我们从i2c缓冲区读取内容,然后将字节值转换为一个角度.您还将看到我们正在使用一种称为卡尔曼滤波器的东西.这将减少指南针传感器返回的噪声量(下一节中将对此进行更多说明).(Here you see us reading from the i2c buffer and then converting the byte values into an angle. You’ll also see that we’re using something called a Kalman filter. This will reduce the amount of noise returned by the compass sensor (More on this in the next section).)
筛选指南针值(Filtering Compass Values)
由于受到本地电磁源和身体运动的干扰,罗盘值固有地具有噪声.您可能会注意到,在后续读取之后,您的值并不会保持恒定,尽管它应该是相当一致的,并且值之间不会有大的跳跃.但是,最好随着时间的推移对这些值进行平滑处理,以获得更清晰的读数.可以使用多个过滤器,但是我选择了一个简单的卡尔曼过滤器,可以找到原始的含义(The compass values are inherently noisy, due to interference from local electromagnetic sources and physical motion. You may notice that your value does not remain constant after subsequent reads, though it should be fairly consistent without large jumps between values. However, it’s better to smooth these values over time for a cleaner reading. There are multiple filters that could be used but I chose a simple kalman filter the original implimentation can be found) 这里.(here.) 引用:https://en.wikipedia.org/wiki/Kalman_filter(Quote: https://en.wikipedia.org/wiki/Kalman_filter)**卡尔曼滤波(Kalman filtering)**也称为(, also known as)线性二次估计(linear quadratic estimation)((()质量保证(LQE)), 是一个(), is an) 算法(algorithm) 使用随时间推移观察到的一系列测量值,其中包括(that uses a series of measurements observed over time, containing) 统计噪声(statistical noise) 和其他不准确性,并通过使用(and other inaccuracies, and produces estimates of unknown variables that tend to be more precise than those based on a single measurement alone, by using) 贝叶斯推理(Bayesian inference) 并估计(and estimating a) 联合概率分布(joint probability distribution) 每个时间范围内的变量.过滤器的名称(over the variables for each timeframe. The filter is named after) 鲁道夫`卡尔曼(Rudolf E. Kálmán) ,其理论的主要开发者之一.(, one of the primary developers of its theory.) 筛选(Filtering) 下图显示了过滤后的值相对于原始值的外观.较深的线表示从传感器返回的噪声值,较浅的线表示平滑的值.如您所见,平滑后的值将比未过滤后的值更合理.过滤返回的值随着时间的推移趋于统一,请务必记住,我们正在预测值应该在哪里,并且它可能会稍微落后于原始值.这意味着在处理过程中获得目标值"修复"的时间可能会稍长一些.(Below is a graph that shows how the filtered values look in relation to the raw values. The darker line represents the noisy value returned from the sensor and the lighter line represents the smoothed values. As you can see the smoothed values will be more consitent than the unfiltered values. The values returned by filtering tend to be more uniform over time, it’s important to keep in mind that we’re making an prediction about where our value should be and it can slightly lag behind your raw value. This means that it might take slightly longer to get a “fix” on your target value while processing.)
卡尔曼(Kalman.h)
不需要为此创建包装器类,因为我们只存储一些值然后更新它们.我们有一个存储状态的结构和两个函数(There was no need to make a wrapper class for this since we’re just storing a few values and then updating them. We have a struct that stores our state and two functions a) kalman_init(...)
和一个(and a) kalman_update(...)
#ifndef __KALMAN_H__
#define __KALMAN_H__
typedef struct {
float q; //process noise covariance
float r; //measurement noise covariance
float x; //value
float p; //estimation error covariance
float k; //kalman gain
} kalman_state;
kalman_state kalman_init(float q, float r, float p, float x);
void kalman_update(kalman_state *s, float m);
#endif // __KALMAN_H__
初始化卡尔曼滤波器(Initializing the Kalman Filter)
初始化非常简单,我们创建一个新的kalman状态结构,然后将状态值设置为传递给函数的值.我们传入过程噪声协方差((It’s pretty simple to initialize, we create a new kalman state struct and then just set the values of the state to those passed into the function. We pass in a Process Noise Covariance () q
),测量噪声协方差((), Measurement Noise Covariance () r
),估计误差协方差((), Estimation Error Covariance () p
)和初始值((), and an initial value () x
)())
kalman_state kalman_init(float q, float r, float p, float x) {
kalman_state s;
s.q = q;
s.r = r;
s.p = p;
s.x = x;
return s;
}
在我们的控制循环中,我们将使用以下代码行初始化卡尔曼滤波器.我在这里选择的值是估计值,我只是对其进行了调整,直到返回的值看起来相当平稳为止.(In our control loop we will initialize the kalman filter using the following lines of code. The values I chose here were estimates, I just adusted them until the values returned seemed reasonably smooth.)
/* Initialize Kalman filter on first call only */
if(init)
{
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
更新卡尔曼滤波器(Updating the Kalman Filter) 要进行更新,我们通过指南针传递先前的状态和新的测量值.首先,我们需要根据较早的状态进行预测,然后更新测量值.(To do an update, we pass in the previous state and the new measured value from the compass. First we need to make a prediction based on earlier states and then we update the measurement values.)
void kalman_update(kalman_state * s, float m) {
//prediction update
//omit x = x
s->p = s->p + s->q;
//measurement update
s->k = s->p / (s->p + s->r);
s->x = s->x + s->k * (m - s->x);
s->p = (1 - s->k) * s->p;
}
最后,在我们的主控制循环中,每当我们更新指南针时,我们都会调用卡尔曼滤波器的更新函数.(Finally, in our main control loop we call the update function for the kalman filter everytime we update the compass.)
/* Update Filtered Bearing */
kalman_update(&state, compass_sensor->bearing);
/* Store Filtered Bearing */
float filtered_bearing = state.x;
就是这样,我们现在准备将所有内容放在一起,并获得最终的True North方位.(That’s it, we’re now ready to put it all together and get our final Bearing for True North.)
把它放在一起(Putting It Together)
一旦所有组件准备就绪,我们现在就可以构建主控制循环.在此示例中,我只是使用无限循环将值打印到屏幕上.您可能会想为自己的目的创建一个适时的控制循环.(Once all of the components are ready we can now build our main control loop. In this example I’m just printing the values to screen using an infinite loop. You’ll likely want to create a well timed control loop for your purposes.)
初始化(Initialization)
我们首先创建并初始化GPS,WMM,指南针和卡尔曼滤波器组件.接下来我们称之为(We start by creating and initializing the GPS, WMM, Compass and Kalman filter components. Next we call any) start()
进入主循环之前的方法.(methods before entering the main loop.)
/* Setup Kalman Filter */
bool init = true;
kalman_state state;
/* Create Components */
gps * gps_sensor = new gps();
wmm * magnetic_model = new wmm();
compass * compass_sensor = new compass();
/* Initialize Components */
gps_sensor->start();
compass_sensor->start();
控制回路(Control Loop) 一旦我们进入主控制回路,您只需更新GPS传感器,将经度,纬度和高度值传递给世界磁模型.现在,我们从指南针中读取信息,初始化卡尔曼滤波器,然后对值进行滤波.(Once we enter the main control loop you just update the GPS sensor, pass longitude, latitude and altitude values to the world magnetic model. Now we read from the compass, intialize the kalman filter and then filter the values.)
/* Enter Main Control Loop */
while (1)
{
/* Update GPS Sensor */
gps_sensor->update();
/* Pass GPS Data to WMM to get Declination */
magnetic_model->update(gps_sensor->gps_lat, gps_sensor->gps_lon, gps_sensor->gps_alt);
/* Read from Compass Sensor */
compass_sensor->update();
/* Initialize the Kalman filter */
if (init)
{
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
/* Update Filtered Bearing */
kalman_update(&state, compass_sensor->bearing);
/* Store Filtered Bearing */
float filtered_bearing = state.x;
}
真北(True North) 一旦获得滤波后的指南针值,我们便可以使用它来使用以下代码行确定到轴承的偏移量.这是一个非常简单的计算,您只需将磁偏角偏移量添加到过滤后的方位角值即可.(Once we have obtained the filtered compass value we can then use it to determine the offset to our bearing using the following line of code. It’s a very simple calculation, you just add the declination offset to your filtered bearing value.)
float true_bearing = filtered_bearing + magnetic_model->declination();
打印值(Printing Values) 我只是在本演示中将值打印到屏幕上.但是,就我而言,我使用这些值来帮助控制伺服电机来控制望远镜的声像.您可能希望使用这些值来控制设备,而不仅仅是打印值,在这里您将执行所需的任何其他步骤.(I’m just printing the values to the screen in this demo. However, in my case I use these values to help control a servo motor to control the pan of my telescope. You would want to use these values to control your device instead of just printing values, this is where you would perform any additional steps needed.)
/* Reset Terminal Output */
printf("\033[0;0H");
/* Print GPS Coordinates */
printf("Latitude: %3d° %2d' %2.3f\" %c Longitude: %3d° %2d' %2.3f\" %c\n\r",
gps_sensor->latitude_degrees(),
gps_sensor->latitude_minutes(),
gps_sensor->latitude_seconds(),
gps_sensor->gps_lat >= 0 ? 'N' : 'S',
gps_sensor->longitude_degrees(),
gps_sensor->longitude_minutes(),
gps_sensor->longitude_seconds(),
gps_sensor->gps_lon >= 0 ? 'E' : 'W');
/* Print Altitude and Declination */
printf("Altitude: %2.3f Declination: %2.3f",
gps_sensor->gps_alt),
magnetic_model->declination());
/* Print Raw Bearing, Filtered Bearing and True Bearing */
printf("Magnetic Bearing: %2.3f Filtered Bearing %2.3f True Bearing %2.3f\n\r",
compass_sensor->bearing,
filtered_bearing,
true_bearing);
打扫干净(Cleaning Up) 退出主循环后,您要记住要停止GPS并清理组件.(Once you exit the main loop you want to remember to stop the GPS and cleanup your components.)
/* Stop GPS sensor */
gps_sensor->stop();
/* Cleanup Components */
delete gps_sensor;
delete compass_sensor;
delete magnetic_model;
完整的例子(Complete Example)
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "wmm.h"
#include "compass.h"
#include "gps.h"
#include "kalman.h"
int main()
{
bool init = true;
kalman_state state;
printf("starting...\n\r");
/* Create Components */
gps * gps_sensor = new gps();
wmm * magnetic_model = new wmm();
compass * compass_sensor = new compass();
/* Initialize Components */
gps_sensor->start();
compass_sensor->start();
/* Clear Screen */
printf("\033[2J\033[?25l");
/* Enter Main Control Loop */
while (1) {
/* Update GPS Sensor */
gps_sensor->update();
/* Pass GPS Data to WMM to get Declination */
magnetic_model->update(gps_sensor->gps_lat, gps_sensor->gps_lon, gps_sensor->gps_alt);
/* Read from Compass Sensor */
compass_sensor->update();
if (init) {
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
kalman_update(&state, compass_sensor->bearing);
float filtered_bearing = state.x;
/* Calculate True Bearing from compass bearing and WMM Declination */
float true_bearing = filtered_bearing + magnetic_model->declination();
/* Reset Terminal Output */
printf("\033[0;0H");
/* Print GPS Coordinates */
printf("Latitude: %3d° %2d' %2.3f\" %c\n\rLongitude: %3d° %2d' %2.3f\" %c\n\r",
gps_sensor->latitude_degrees(),
gps_sensor->latitude_minutes(),
gps_sensor->latitude_seconds(),
gps_sensor->gps_lat >= 0 ? 'N' : 'S',
gps_sensor->longitude_degrees(),
gps_sensor->longitude_minutes(),
gps_sensor->longitude_seconds(),
gps_sensor->gps_lon >= 0 ? 'E' : 'W');
/* Print Altitude and Declination */
printf("Altitude: %2.3f\n\r\n\rDeclination: %2.3f\n\r",
gps_sensor->gps_alt,
magnetic_model->declination());
/* Print Raw Bearing, Filtered Bearing and True Bearing */
printf("\n\rMagnetic Bearing: %2.3f\n\rFiltered Bearing: %2.3f\n\rTrue Bearing %2.3f\n\r",
compass_sensor->bearing,
filtered_bearing,
true_bearing);
}
gps_sensor->stop();
delete gps_sensor;
delete compass_sensor;
delete magnetic_model;
return 0;
}
使用代码(Using the Code)
如果您按照本文开头的图示和软件先决条件设置了硬件,则可以使用以下bash命令下载并运行示例.(If you’ve set up your hardware following the diagram and the software prerequisites at the begging of the article you can download and run the sample using the following bash commands.)
git clone https://github.com/GalacticSoft/Starbot-Command.git
cd Starbot-Command/src
make clean
make demo
sudo ./demo
就是这样,如果演示正常运行,您应该看到将坐标打印到屏幕上,如下面的屏幕截图所示.(That’s it, if the demo works you should see your coordinates printed to the screen like the screenshot below.)
现在您就可以开始将地理位置和罗盘方位用于您的机器人或IoT设备了,尽情享受吧!(Now you’re ready to start using geographic location and compass bearings with your robot or IoT device, enjoy!)
许可
本文以及所有相关的源代码和文件均已获得The Code Project Open License (CPOL)的许可。
C++ Raspberry Linux Dev IoT 新闻 翻译