一、卡尔曼滤波原理
1、什么是卡尔曼滤波?
你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,卡尔曼滤波总是能指出真实发生的情况。
在连续变化的系统中使用卡尔曼滤波是非常理想的,它具有占用内存小的优点(除了前一个状态量外,不需要保留其它历史数据),并且速度很快,很适合应用于实时问题和嵌入式系统。
在Google上找到的大多数关于实现卡尔曼滤波的数学公式看起来有点晦涩难懂,这个状况有点糟糕。实际上,如果以正确的方式看待它,卡尔曼滤波是非常简单和容易理解的,下面我将用漂亮的图片和色彩清晰的阐述它,你只需要懂一些基本的概率和矩阵的知识就可以了。
2、我们能用卡尔曼滤波做什么?
用玩具举例:你开发了一个可以在树林里到处跑的小机器人,这个机器人需要知道它所在的确切位置才能导航。
注意这个状态只是关于这个系统基本属性的一堆数字,它可以是任何其它的东西。在这个例子中是位置和速度,它也可以是一个容器中液体的总量,汽车发动机的温度,用户手指在触摸板上的位置坐标,或者任何你需要跟踪的信号。
这个机器人带有GPS,精度大约为10米,还算不错,但是,它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖,如果机器人走错了一步,就有可能掉下悬崖,所以只有GPS是不够的。
或许我们知道一些机器人如何运动的信息:例如,机器人知道发送给电机的指令,知道自己是否在朝一个方向移动并且没有人干预,在下一个状态,机器人很可能朝着相同的方向移动。当然,机器人对自己的运动是一无所知的:它可能受到风吹的影响,轮子方向偏了一点,或者遇到不平的地面而翻倒。所以,轮子转过的长度并不能精确表示机器人实际行走的距离,预测也不是很完美。
GPS 传感器告诉了我们一些状态信息,我们的预测告诉了我们机器人会怎样运动,但都只是间接的,并且伴随着一些不确定和不准确性。但是,如果使用所有对我们可用的信息,我们能得到一个比任何依据自身估计更好的结果吗?回答当然是YES,这就是卡尔曼滤波的用处。
3、卡尔曼滤波是如何看到你的问题的
下面我们继续以只有位置和速度这两个状态的简单例子做解释。
我们并不知道实际的位置和速度,它们之间有很多种可能正确的组合,但其中一些的可能性要大于其它部分:
在上图中,位置和速度是不相关的,这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣:位置和速度是相关的,观测特定位置的可能性取决于当前的速度:
这种情况是有可能发生的,例如,我们基于旧的位置来估计新位置。如果速度过高,我们可能已经移动很远了。如果缓慢移动,则距离不会很远。跟踪这种关系是非常重要的,因为它带给我们更多的信息:其中一个测量值告诉了我们其它变量可能的值,这就是卡尔曼滤波的目的,尽可能地在包含不确定性的测量数据中提取更多信息!
4、使用矩阵来描述问题
当然,在这里我们只用到了位置和速度,实际上这个状态可以包含多个变量,代表任何你想表示的信息。接下来,我们需要根据当前状态(k-1 时刻)来预测下一状态(k 时刻)。记住,我们并不知道对下一状态的所有预测中哪个是“真实”的,但我们的预测函数并不在乎。它对所有的可能性进行预测,并给出新的高斯分布。
它将我们原始估计中的每个点都移动到了一个新的预测位置,如果原始估计是正确的话,这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢?下面用一个基本的运动学公式来表示:
5、外部控制量
我们并没有捕捉到一切信息,可能存在外部因素会对系统进行控制,带来一些与系统自身状态没有相关性的改变。
以火车的运动状态模型为例,火车司机可能会操纵油门,让火车加速。相同地,在我们机器人这个例子中,导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息,我们可
6、外部干扰
如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的,则不会出现什么问题。
但是,如果存在未知的干扰呢?例如,假设我们跟踪一个四旋翼飞行器,它可能会受到风的干扰,如果我们跟踪一个轮式机器人,轮子可能会打滑,或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪,如果没有把这些外部干扰考虑在内,我们的预测就会出现偏差。
在每次预测之后,我们可以添加一些新的不确定性来建立这种与“外界”(即我们没有跟踪的干扰)之间的不确定性模型:
这产生了具有不同协方差(但是具有相同的均值)的新的高斯分布。
7、用测量值来修正估计值
我们可能会有多个传感器来测量系统当前的状态,哪个传感器具体测量的是哪个状态变量并不重要,也许一个是测量位置,一个是测量速度,每个传感器间接地告诉了我们一些状态信息。
我们可以计算出传感器读数的分布,用之前的表示方法如下式所示:
卡尔曼滤波的一大优点就是能处理传感器噪声,换句话说,我们的传感器或多或少都有点不可靠,并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。
从测量到的传感器数据中,我们大致能猜到系统当前处于什么状态。但是由于存在不确定性,某些状态可能比我们得到的读数更接近真实状态。
现在我们有了两个高斯分布,一个是在预测值附近,一个是在传感器读数附近。
我们必须在预测值(粉红色)和传感器测量值(绿色)之间找到最优解。
剩下的就是重叠部分了,这个重叠部分的均值就是两个估计最可能的值,也就是给定的所有信息中的最优估计。
瞧!这个重叠的区域看起来像另一个高斯分布。
如你所见,把两个具有不同均值和方差的高斯分布相乘,你会得到一个新的具有独立均值和方差的高斯分布!下面用公式讲解。
8、融合高斯分布
将所有公式整合起来
9、总结
以上所有公式中,你只需要用到式(7)、(18)、(19)。(如果忘了的话,你可以根据式(4)和(15)重新推导一下)
我们可以用这些公式对任何线性系统建立精确的模型,对于非线性系统来说,我们使用扩展卡尔曼滤波,区别在于EKF多了一个把预测和测量部分进行线性化的过程。
二、打造STM32F0 USB开发板
刚完成的STM32F072 USB开发板,使用48脚LQFP的STM32F072C8T6,也可以使用其它封装兼容的带USB型号,甚至是M3系的STM32F103C8T6这种。上半年从论坛买了块STM32F091 Nucleo, 但是不带USB,所以为了学习USB自己做一块咯。下面是电路图,除了一片1117 3.3V LDO,外围器件少到极致了吧,晶振不用的话是可以不装的。板子可以直接通过 USB mini口供电。外围引出的插针有一路 SPI, 一路 I2S, 一路 UART, 一路 I2C, 一路 8-bit GPIO, 一路 UART/I2C共用,以及几个零星的GPIO。这些已方便开发简单的USB设备了。
PCB layout 示意图
好,STM32F072 10块钱以内就可以搞定,整个开发板成本很低了吧。如果你有ST-Link, 或者是带有ST-Link的STM32 Discovery/Nucleo开发板,用SWD调试线连上就可以下载程序了。如果没有ST-Link, 还可以从串口下载程序,只需要把BOOT0跳线接上即可,因为STM32内带了Bootloader. 如果连串口线都没有?呵呵,要是像F072这样带USB的,还可以从USB直接下载,别的硬件也省了,怎么样,够简吧?
OK,来写第一个测试程序:定时控制LED闪烁。
#include "stm32f0xx.h"
int main(void)
{
RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable GPIO port A & B clock
GPIOA->MODER = GPIO_MODER_MODER8_0; // PA8 as general output (LED)
RCC->APB1ENR |= RCC_APB1ENR_TIM6EN; // enable basic timer 6
TIM6->PSC = 9999; // prescaler
TIM6->ARR = 399; // auto reload value
TIM6->CR1 = TIM_CR1_URS|TIM_CR1_CEN; // start counter
while(1)
{
static char a=0;
if(TIM6->SR & TIM_SR_UIF) // check if overflow
{
TIM6->SR &= ~TIM_SR_UIF; // clear flag
if(a==0)
{
GPIOA->BSRR = (1<<8);
a=1;
}
else
{
GPIOA->BRR = (1<<8);
a=0;
}
}
}
}
上面这个程序所做的事情,先是初始化GPIO, 设置PA8为输出口(板子上连了一个LED),然后是设置定时器Timer 6, 这是一个自动重装的计数器,我把它调到0.5秒中溢出一次。在下面的循环里面,就是检测溢出标志,然后切换LED的亮和灭状态。学过C语言的,都应该看得懂;至于RCC, GPIOA, TIM6 这几个结构指针的定义,都在#include的头文件里面,这是和硬件相关的,具体请查阅"RM0091 STM32F0x1/STM32F0x2/STM32F0x8 Reference Manual"编程手册。
如何编译上面这个 C 程序,且听下回分解。这里暂且假定编译成功了,得到一个 HEX 文件,也就是要烧写的二进制代码。
如果你是使用KEIL, IAR等集成开发环境,那么用自带的烧写工具就可以进行写入了。如果是像我cruelfox这样追求精简,仅使用GCC命令行工具的,就需要再找下载程序用的软件了。
如果是使用ST-Link,可以使用ST自己的STVP (Visual Programmer),这个东东在ST网站上可以下载到,不过是包含在九十兆左右的一个大包"ST Toolset"里面。(下载URL http://www.st.com/web/catalog/tools/FM147/CL1794/SC961/SS1533/PF210568 )这个软件的界面是这个样子的:
主菜单上面 Erase, Program, Verify, Read 功能很明了了,File-->Open可以加载HEX文件。第一次运行STVP的时候,要选择ST-LINK调试器,和 SWD接口。
如果没有ST-Link, 使用串口下载的话,需要"Flash Loader Demostrator"软件,这个也可以从ST网站直接下载(URL http://www.st.com/st-web-ui/static/active/en/st_prod_software_internet/resource/technical/software/demo_and_example/stsw-mcu005.zip)。下载前要把BOOT0跳线接上,使STM32进入Bootloader模式,USART1连接到PC的串口(我用的是FT232RL USB转串口),把MCU加电。运行软件,界面是这样的:
选择串口,然后点"Next",如果成功连上了,则界面变成下面这样
点"Next"继续
这时已显示出识别出的STM32型号,点"Next"到下一步进行具体的操作。
OK, 下载HEX,擦除,上载(读Flash内容) 功能都一看就明白了吧。
第三种下载方式,从USB,需要ST的"DFUSe Demo"软件,也是从ST网站下载的(URL http://www.st.com/st-web-ui/static/active/en/st_prod_software_internet/resource/technical/software/demo_and_example/stsw-stm32080.zip)。也需要把BOOT0跳线接上,还必须连接USB口,然后PC提示找到了新硬件。安装好驱动以后,再启动软件,界面如下:
不过现在不能把HEX文件直接写入,而需要先生成dfu文件,使用一起安装得到的"DFU File Manager"程序,从HEX生成dfu.
至于 VID, PID 我还是保留和原来的一致,不然得重新安装驱动(为什么要使用DFU文件我还没理解清楚)。得到dfu文件就可以用上面的软件烧写了。
怎么样,我的开发板够精简吧?
STM32F0xx 系列是ARM Cortex-M0架构,地址空间32位,也就是4G Bytes的访问范围。数据和代码使用同一编址,下图是地址空间的布局:
实际上单片机用到的资源很少,地址空间大部分都没有内容。我使用的STM32F072C8T6带有64kB的Flash ROM, 16kB的SRAM,起始分别是 0x08000000 和 0x20000000. (由于有硬件映射功能,在0x00000000也就是最低地址,还可以访问ROM或者RAM的内容). 单片机片上外设的寄存器,则分布在更高的地址空间。读写这些寄存器,在CPU看来和读写内存(RAM)操作是一样的。
所以,C语言访问设备寄存器,和访问内存中的一个变量一样。只要知道寄存器的地址,通过一个指针访问就可以实现读写。上一贴子我的程序中引用了 RCC, GPIOA, TIM6 这三个(结构)指针,它们的值(也就是地址)以及类型(代表访问的内容)定义在 stm32f0xx.h 这个头文件中。因为设备寄存器太多了哇,如果每一个都定义一个指针就太烦琐了,所以把按功能划分定义成组,每组用一个C语言的结构类型表示,写起来也更清晰。而寄存器里面的位描述也可以定义成一些宏,在读程序的时候就知道是什么意思了。如果有兴趣,可以把 stm32f0xx.h 文件和STM32F0的手册对照着阅读。
好,假设已经熟悉寄存器操作了,知道怎么配置寄存器实现想要的功能,那么就可以写C程序让STM32工作了。现在需要一个工具来将C程序翻译成机器代码——编译器,或者是叫做工具链(Tool chain)。Keil MDK-ARM 或者 IAR-EWARM 开发环境都带有各自的编译器,不过我更偏向于用开源的GCC-ARM. 在launchpad.net上可以下载到编译好的arm-gcc工具链zip包,将它解压缩,加到PATH里面就可以直接用了,很方便(很精简吧)。
OK,现在来编译上面那个mini.c文件,命令行:
arm-none-eabi-gcc -c -Os -mcpu=cortex-m0 -mthumb mini.c
gcc的参数 -c 是表示仅编译,-Os 是优化代码大小,-mcpu=cortex-m0 -mthumb 是指定指令集的,因为ARM有不同的版本。对了,include的头文件还没弄到呢。要编译通过需要把 stm32f0xx.h 这个文件找来。我的建议是下载ST提供的 "STM32F0x2 USB FS Device Library" 程序库(URL http://www.st.com/st-web-ui/static/active/en/st_prod_software_internet/resource/technical/software/firmware/stsw-stm32092.zip),把里面需要的头文件等等扒出来。在 stm32f0xx.h 中还包含了另外几个头文件,一并弄出来放到工程目录下。
如果编译成功,将得到 mini.o 目标文件。可以用 arm-none-eabi-objdump -S mini.o 反汇编看看翻译成什么代码了。
E:\arm\test072\mini>arm-none-eabi-objdump -S mini.o
mini.o: file format elf32-littlearm
Disassembly of section .text.startup:
00000000 <main>:
0: 4b16 ldr r3, [pc, #88] ; (5c <main+0x5c>)
2: 2280 movs r2, #128 ; 0x80
4: 6959 ldr r1, [r3, #20]
6: 0292 lsls r2, r2, #10
8: 430a orrs r2, r1
a: b510 push {r4, lr}
c: 2180 movs r1, #128 ; 0x80
e: 615a str r2, [r3, #20]
24: 851a strh r2, [r3, #40] ; 0x28
26: 2290 movs r2, #144 ; 0x90
28: 32ff adds r2, #255 ; 0xff
2a: 62da str r2, [r3, #44] ; 0x2c
50: 6184 str r4, [r0, #24]
52: 1c0a adds r2, r1, #0
54: e7ee b.n 34 <main+0x34>
56: 8504 strh r4, [r0, #40] ; 0x28
58: 2200 movs r2, #0
5a: e7eb b.n 34 <main+0x34>
5c: 40021000 .word 0x40021000
60: 40001000 .word 0x40001000
64: 0000270f .word 0x0000270f
68: 00000000 .word 0x00000000
如上,其实里面就一个main函数。但是 main 的入口地址还没有确定,而且它还使用了一个static型的内存变量,地址也还没有确定。可以用 arm-none-eabi-nm mini.o 来查看模块里面的全局符号表:
E:\arm\test072\mini>arm-none-eabi-nm mini.o
00000000 b a.4686
00000000 T main
那么,怎么让程序放到ROM中合适的地址,并运行呢?如果熟悉C语言编程就知道还有一步——链接,才能确定符号的地址。但是,到目前为止我们还没有告诉GCC地址的布局,也就是RAM从哪里开始,代码放在哪里。因为ARM的器件很多,这并不是统一的,所以需要提供一些信息给链接程序。具体地,需要一个Linker Script, 可以从软件包中找到 STM32F072C8_FLASH.ld (或者用近似的来修改得到)
/*
*****************************************************************************
** File : stm32_flash.ld
*****************************************************************************
*/
/* Entry Point */
ENTRY(Reset_Handler)
/* Highest address of the user mode stack */
_estack = 0x20003FFF; /* end of RAM */
/DISCARD/ :
{
libc.a ( * )
libm.a ( * )
libgcc.a ( * )
}
.ARM.attributes 0 : { *(.ARM.attributes) }
}
OK,下面就是链接了,使用命令 arm-none-eabi-ld mini.o -Le:\arm-2014q3\arm-none-eabi\lib\armv6-m -Le:\arm-2014q3\lib\gcc\arm-none-eabi\4.8.4\armv6-m -T STM32F072C8_FLASH.ld -o mini.elf
这里面 -L 参数是添加标准库文件的搜索路径,虽然暂时并没有用到C标准库里面的东西,但是Linker Script里面引用了标准库文件。-o 指定输出的目标文件。这么就快要得到最终的机器码了,不过好象还缺少了什么……
arm-none-eabi-ld: warning: cannot find entry symbol Reset_Handler; defaulting to 08000000
linker给了一个警告:找不到入口地址 Reset_Handler 的值,设成了默认 0x08000000. 下面再用objdump -S反汇编看一下
E:\arm\test072\mini>arm-none-eabi-objdump -S mini.elf
mini.elf: file format elf32-littlearm
Disassembly of section .text:
08000000 <main>:
8000000: 4b16 ldr r3, [pc, #88] ; (800005c <main+0x5c>)
8000002: 2280 movs r2, #128 ; 0x80
8000004: 6959 ldr r1, [r3, #20]
8000006: 0292 lsls r2, r2, #10
8000008: 430a orrs r2, r1
800000a: b510 push {r4, lr}
8000050: 6184 str r4, [r0, #24]
8000052: 1c0a adds r2, r1, #0
8000054: e7ee b.n 8000034 <main+0x34>
8000056: 8504 strh r4, [r0, #40] ; 0x28
8000058: 2200 movs r2, #0
800005a: e7eb b.n 8000034 <main+0x34>
800005c: 40021000 .word 0x40021000
8000060: 40001000 .word 0x40001000
8000064: 0000270f .word 0x0000270f
8000068: 20000000 .word 0x20000000
现在 main() 被放到ROM最开始去了,这好象是对的?如果了解ARM Cortex-M0下就知道这样错了,因为最开始应该是中断向量表。我们还没有编写Linker Script中的 .isr_vectors 段的内容。而且,一上来初始化堆栈指针等工作都没有做就直接运行 main() 了也不合适吧?还缺少了初始化代码。
在软件包中搜刮一个 startup_stm32f072.s 汇编文件
/**
******************************************************************************
* @file startup_stm32f072.s
* @author MCD Application Team
******************************************************************************
*/
.syntax unified
.cpu cortex-m0
.fpu softvfp
.thumb
.global g_pfnVectors
.global Default_Handler
.weak USART2_IRQHandler
.thumb_set USART2_IRQHandler,Default_Handler
.weak USART3_4_IRQHandler
.thumb_set USART3_4_IRQHandler,Default_Handler
.weak CEC_CAN_IRQHandler
.thumb_set CEC_CAN_IRQHandler,Default_Handler
.weak USB_IRQHandler
.thumb_set USB_IRQHandler,Default_Handler
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
原来是这样,中断向量表在这里进行了描述,还有设置堆栈,初始化全局变量的代码,然后跳转到 main 执行。好了,这样就该差不多了。这个汇编程序是GNU AS的语法,可以用 arm-none-eabi-gcc 来直接汇编
arm-none-eabi-gcc -c startup_stm32f072.s
链接两个目标模块
arm-none-eabi-ld mini.o startup_stm32f072.o -Le:\arm-2014q3\arm-none-eabi\lib\armv6-m -Le:\arm-2014q3\lib\gcc\arm-none-eabi\4.8.4\armv6-m -T STM32F072C8_FLASH.ld -o mini.elf
最后转换出一个 HEX 文件
arm-none-eabi-objcopy -Oihex mini.elf mini.hex
可以进行烧写了。
我这个是最简化的例子,使用最简化的软件工具,不过已经包含了基本的C语言框架。
第一个USB工程例子:USB存储盘。因为手边没有SPI或I2C接口的Flash, 就权且用单片机的RAM虚拟一下吧。这个"U盘"容量标成160kB, 实际上有效的存储只有10kB,用一点扇区映射的技巧骗过操作系统,当然存放文件只能几kB了。
ST官方提供了一个 STM32F0 的USB固件库,(URL http://www.st.com/st-web-ui/static/active/en/st_prod_software_internet/resource/technical/software/firmware/stsw-stm32092.zip),包括了硬件库、核心库和类库的C头文件和源文件。我cruelfox琢磨了一整天,还是没搞清楚怎么调用这些函数,只好照着个Example改吧。
于是就用MassStorage类的例子来下手了,这个目录里面的主文件 app.c 非常简单
/**
* @file app.c
*/
/* Includes ------------------------------------------------------------------*/
#include "usbd_msc_core.h"
#include "usbd_usr.h"
USB_CORE_HANDLE USB_Device_dev ;
int main(void)
{
USBD_Init(&USB_Device_dev,
&USR_desc,
&USBD_MSC_cb,
&USR_cb);
while (1)
{
}
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t* file, uint32_t line)
{
while (1)
{}
}
#endif
一切USB相关的东西都在 USBD_init() 这个函数里面了。追查源代码,可以发现这个函数调用其实是向库函数提供了一堆回调函数的接口,也就是说写一些子程序,让驱动程序在需要的时候调用。这样开发USB的我们就不用去管中断什么的了,要做的就是实现各种USB的请求——其实程序库里面已经把大部分的请求应答都实现了,而具体的类库(比如我这里用的MassStorage类)又处理了剩余的大部分,所以只剩下功能性的需要自己写。
在 usbd_desc.c 这个程序里面可以看到描述符是怎么创建和返回的
/**
******************************************************************************
* @file usbd_desc.c
* @author MCD Application Team
* @version V1.0.0
* @date 31-January-2014
* @brief This file provides the USBD descriptors and string formating method.
******************************************************************************
* @attention
*
* <h2><center>© COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
value = value << 4;
pbuf[ 2* idx + 1] = 0;
}
}
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
插一句: 开发USB设备,有很重要的一部分是编写描述符(当然也要借用已有的模板),设备叫什么,功能是什么等等都从描述符体现。只要设备向主机成功返回了描述符,主机就会提示找到硬件。
设计到U盘读写操作的部分在 usbd_storage_msd.c 里面,我就是在例子程序上改的
/**
******************************************************************************
* @file usbd_storage_msd.c
* @author MCD application Team
* @version V1.0.0
* @date 31-January-2014
* @brief This file provides the disk operations functions.
{
return (STORAGE_LUN_NBR - 1);
}
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
主要是需要完成 报告容量、读、写 三个函数。如果有可实现存储的硬件,接口连起来即可。我上面是用了10kB的RAM来存放,动态分配20个扇区来存储。如果检测到要写的整个扇区都是0,那么就忽略。于是这样就做成了虚拟的U盘。
附件包含了所有需要的程序源代码,USB库的部分在lib子目录下面。build.bat 中是编译的命令。依靠这个USB函数库开发起来还算不难吧,只要看懂了例子,mass storage class还比较简单,因为硬件涉及就是读扇区、写扇区。USB HID设备五花八门,就不是这么简单了,继续研究去。