nsh：nuttx shell。串口终端打印。
uorb：应该是进程间通讯的核心？
mavlink:应该是串口通讯？
EKF：是卡尔曼滤波，整合陀螺仪和加速度计，罗盘数据的。
AHRS:航子系统。attitude heading reference system 姿态和方位参照系统
UAV：unmanned aerial vehicle 无人驾驶航空器;
DCM:Direction Cosine Matrix 方向余弦矩阵
HIL:HIL硬件在环(hardware-in-the-loop，HIL)仿真被证明是一种有效的解决方法。该技术能确保在开发周期早期就完成嵌入式软件的测试。
RMS---root meam square.均方根误差
rpy：roll pitch yaw
TAS：即真空速（True Air Speed），即真实空速，是表示飞行器飞行时相对于周围空气运动的速度，其缩写形式为TAS，用符号VT表示。
hrt_call_every-----hrt:high resolution timer高精度定时器
os_start启动最后调用了：
CONFIG_USER_ENTRYPOINT=nsh_main.实在.config=deconfig里面定义了
所以第一个应用入口是：nsh_main
ardupilot飞控main函数入口：
AP_HAL_MAIN_CALLBACKS(&copter);
#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
int AP_MAIN(int argc, char* const argv[]); \
int AP_MAIN(int argc, char* const argv[]) { \
hal.run(argc, argv, CALLBACKS); \
return 0; \
} \
}
hal在copter.cpp中定义。
nsh_main会去读运行脚本rcS，调用rc.APM启动--Ardupilot。
一个是ardupilot/mk/PX4/ROMFS/init.d里的rcS，另一个是rc.APM，这个脚本在rcS里得到了调用，也就是说，rcS就是为Nuttx的启动文件。
那么到底调用ArduPilot_main的地方在哪里呢？查看rc.APM的最低端：
nsh_main->nsh_consolemain->nsh_initscript->nsh_script->运行rcS->rc.APM->Ardupilot启动运用代码。
###########################################################################################################
###########################################################################################################
在说mavlink之前，先说串口，因为mavlink是用串口通讯的
串口驱动初始化跟随nuttx的：up_serialinit
用uart_register注册设备文件，（file_operations g_serialops）提供应用程序控制。
每一个驱动（up_dev_s g_uart7priv）提供了缓存：
.recv =
{
.size = CONFIG_UART7_RXBUFSIZE,
.buffer = g_uart7rxbuffer,
},
.xmit =
{
.size = CONFIG_UART7_TXBUFSIZE,
.buffer = g_uart7txbuffer,
},
.ops = &g_uart_dma_ops,//真正的操作函数。g_serialops其实就是调用他来完成open，ioctol，等操作的。
流程：在begin(open)的时候调用：up_dma_setup，初始化和开启串口，连接DMA，配置DMA接收中断。DMA接收中断就是拷贝数据到.recv.buffer
ioctl就是配置波特率，crt(cst,rst功能没用)等功能的。
read就是直接读.recv.buffer数据
write就是向.xmit.buffer写如数据，并发送第一个字节，开始发送完成中断。以便将.xmit.buffer数据发送完毕。
###########################################################################################################
在上次有个UARTDriver.cpp提供了对驱动的读写操作：PX4UARTDriver
再用串口管理器AP_SerialManager::init来管理串口。每个串口 协议 和 波特率 也定义了：AP_SerialManager::AP_SerialManager------AP_SerialManager::var_info[]
在init_ardupilot中用SerialProtocol_MAVLink协议来匹配AP_SerialManager中对应的mavlink的协议。
// setup telem slots with serial ports
for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
###########################################################################################################
###########################################################################################################
MAVLink是MCU/IMU间以及Linux进程和地面站链路通信间的主干通信协议。
GCS_MAVLINK_Copter地面站的MAVLink
_MAV_RETURN_uint8_t(msg, x)就是msg->payload64[x]取一个char
以下是mavlink的数据包：
mavlink-v1
MAVPACKED(
typedef struct __mavlink_message {
uint16_t checksum; ///< sent at end of packet
uint8_t magic; ///< protocol magic marker
uint8_t len; ///< Length of payload
uint8_t seq; ///< Sequence of packet
uint8_t sysid; ///< ID of message sender system/aircraft
uint8_t compid; ///< ID of the message sender component
uint8_t msgid; ///< ID of message in payload
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
}) mavlink_message_t;
mavlink-v2
MAVPACKED(
typedef struct __mavlink_message {
uint16_t checksum; ///< sent at end of packet
uint8_t magic; ///< protocol magic marker
uint8_t len; ///< Length of payload
uint8_t incompat_flags; ///< flags that must be understood----------------不兼容标志。最后以为表示有没signature标签数据。
uint8_t compat_flags; ///< flags that can be ignored if not understood
uint8_t seq; ///< Sequence of packet
uint8_t sysid; ///< ID of message sender system/aircraft
uint8_t compid; ///< ID of the message sender component
uint32_t msgid:24; ///< ID of message in payload------------------------message ID变长了。
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
uint8_t ck[2]; ///< incoming checksum bytes
uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];----------------------------标签
}) mavlink_message_t;
下面是心跳包的内容：9个字节。
typedef struct __mavlink_heartbeat_t
{
uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags.*/
uint8_t type; /MAV_TYPE==============*< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)*/
uint8_t autopilot; /MAV_AUTOPILOT=========*< Autopilot type / class. defined in MAV_AUTOPILOT ENUM*/
uint8_t base_mode; /MAV_MODE_FLAG=========*< System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h*/
uint8_t system_status; /MAV_STATE=============*< System status flag, see MAV_STATE ENUM*/
uint8_t mavlink_version; /*< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/
} mavlink_heartbeat_t;
class MAVLink_routing是mavlink的路由器，他会保存20个接收到的设备对象。
_mav_finalize_message_chan_send是发送？
参数发送msgid是22.
###########################################################################################################
###########################################################################################################
mpu6000_main为例：他是一个驱动模块。
rcS->rc.APM->mpu6000 start启动模块。在ioctol里面，开了个定时器来做任务---->(hrt_callout)&MPU6000::measure_trampoline。
Ardupilot是最后一个启动，所以可以死循环。
Sensors.cpp (ardupilot\modules\px4firmware\src\modules\sensors): _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
Ardupilot入口函数定义：
AP_HAL_MAIN_CALLBACKS(&copter);
copter.cpp定义了：
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Copter copter;
HAL_PX4_Class.cpp定义了：
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_PX4 hal_px4;
return hal_px4;
}
所以hal.run运行的就是
void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks) const
g_callbacks = callbacks;说明g_callbacks==copter
###########################################################################################################
###########################################################################################################
编译：
编译从mk/apm.mk开始
mk/board_px4.mk
mk/px4_targets.mk重要。
目标：
###其中$(PX4_ROOT)/Archives/px4fmu-v2.export是编译px4-nuttx, $(SKETCHCPP)生成Ardupilot的module.mk文件。px4-io-v2编译STM32副MCU,包括nuttx和对应的driver
px4-v2: $(BUILDROOT)/make.flags CHECK_MODULES $(MAVLINK_HEADERS) $(PX4_ROOT)/Archives/px4fmu-v2.export $(SKETCHCPP) module_mk px4-io-v2
$(RULEHDR)
$(v) cp $(PX4_V2_CONFIG_FILE) $(PX4_ROOT)/makefiles/nuttx/
@echo =====surpass====== $(PX4_MAKE) px4fmu-v2_APM
//这里就是编译driver层了。
$(PX4_MAKE) px4fmu-v2_APM
$(v) arm-none-eabi-size $(PX4_ROOT)/Build/px4fmu-v2_APM.build/firmware.elf
$(v) cp $(PX4_ROOT)/Images/px4fmu-v2_APM.px4 $(SKETCH)-v2.px4
$(v) $(SKETCHBOOK)/Tools/scripts/add_git_hashes.py $(HASHADDER_FLAGS) "$(SKETCH)-v2.px4" "$(SKETCH)-v2.px4"
$(v) echo "PX4 $(SKETCH) Firmware is in $(SKETCH)-v2.px4"
编译apm的驱动：
modules/PX4Firmware/Makefile.make
在modules/PX4Firmware/makefiles//nuttx中有很多mk文件，这就是相当于目标，他可以转换，如：
modules/PX4Firmware/makefiles//nuttx/config_px4fmu-v2_APM.mk转换为px4fmu-v2_APM
EXPLICIT_CONFIGS就去看带入的参数是否在这里面：px4fmu-v2_APM board_px4io-v2...
然后调用$(EXPLICIT_CONFIGS): all
all: $(DESIRED_FIRMWARES)
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules
$(Q)+ $(MAKE) -r -C $(work_dir) \
-f $(PX4_MK_DIR)firmware.mk \
CONFIG=$(config) \
WORK_DIR=$(work_dir) \
$(FIRMWARE_GOAL)
就等于：
make -r -C /home/supass/workspace/code/ardupilot-1/ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/ -f /home/supass/workspace/code/ardupilot-1/ardupilot/modules/PX4Firmware/makefiles/firmware.mk CONFIG=px4fmu-v2_APM WORK_DIR=/home/supass/workspace/code/ardupilot-1/ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/ firmware
############################################################################################################################
在文件/home/supass/workspace/code/ardupilot-1/ardupilot/modules/PX4Firmware/makefiles/firmware.mk中：
firmware: $(PRODUCT_BUNDLE)
$(PRODUCT_BUNDLE): $(PRODUCT_BIN)
$(PRODUCT_BIN): $(PRODUCT_ELF)
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
现在开始编译各个模块了。
$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
$(MODULE_OBJS): workdir = $(@D)
$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q)+ $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
-C $(workdir) \
MODULE_WORK_DIR=$(workdir) \
MODULE_OBJ=$@ \
MODULE_MK=$(mkfile) \
MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
module
而各个模块目录那里呢：
MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module.pre.o)
MODULE_MKFILES := $(foreach module,$(MODULES),$(call MODULE_SEARCH,$(module)))
意思就是从MODULES里面来而MODULES没在这个文件定义，在他包含的各个.mk文件中定义，包含的mk文件可以用GLOBAL_DEPS += $(MAKEFILE_LIST)得到。
在px4fmu-v2_APM目标中：
GLOBAL_DEPS=modules/PX4Firmware/makefiles/firmware.mk modules/PX4Firmware/makefiles//setup.mk modules/PX4Firmware/makefiles//nuttx/config_px4fmu-v2_APM.mk mk/PX4/px4_common.mk modules/PX4Firmware/makefiles//nuttx/board_px4fmu-v2.mk modules/PX4Firmware/makefiles//toolchain_gnu-arm-eabi.mk modules/PX4Firmware/makefiles//nuttx.mk modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/config.h
其实主要的驱动在modules/PX4Firmware/makefiles/firmware.mk，mk/PX4/px4_common.mk，modules/PX4Firmware/makefiles//nuttx.mk里面定义。
############################################################################################################################
编译模块的makefile：$(PX4_MK_DIR)module.mk====/home/supass/workspace/code/ardupilot-1/ardupilot/modules/PX4Firmware/makefiles/module.mk
每个模块目录中也有个mk文件，被modules/PX4Firmware/makefiles/module.mk包含，以便接收信息：
$(info % MODULE_NAME = $(MODULE_NAME))
$(info % MODULE_SRC = $(MODULE_SRC))
$(info % MODULE_OBJ = $(MODULE_OBJ))
$(info % MODULE_WORK_DIR = $(MODULE_WORK_DIR))
然后就是用MODULE_SRC编译了。
编译骨架已经说完，还有nuttx编译如何拷贝deconfig文件到.config，make.def,envset文件的拷贝，
MODULE_ENTRYPOINT ?= $(MODULE_COMMAND)_main如ardupilot_main作为驱动的入口函数。
###########################################################################################################
###########################################################################################################
//scheduler_tasks任务调度器，就是2.5MS检测一次，到时间了就调用函数。SCHED_TASK(callback函数, 时间间隔(用的是频率), 最大运行时间),
如gcs运行时间用的copter.gcs_out_of_time标志位，时间超了，就return出去了。这是任务不是线程，没有保护现场的机制。
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),//电池和磁罗盘读取。
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 50, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
//gcs_check_input是用来 接收 并 处理 地面站数据。2.5ms一次。
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),//心跳包1一秒发一次
//每20ms，检测一次延时发送的数据，将他们发出去。MSG_RETRY_DEFERRED并不是发送的命令
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),//发参数是收到命令才发，其他状态定时发。
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(update_trigger, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK(dataflash_periodic, 400, 300),
SCHED_TASK(perf_update, 0.1, 75),
SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),//100ms去查询一次校准情况。真正校准是mavlink下发命令，在采集(fast_loop)中去集合采样，计算校准值。
#if ADSB_ENABLED == ENABLED
SCHED_TASK(adsb_update, 1, 100),
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
SCHED_TASK(frsky_telemetry_send, 5, 75),
#endif
SCHED_TASK(terrain_update, 10, 100),
#if EPM_ENABLED == ENABLED
SCHED_TASK(epm_update, 10, 75),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
};
###########################################################################################################
###########################################################################################################
模块ms5611：入口
int ms5611_main(int argc, char *argv[])
选择SPI：MS5611_SPI
读取计算参数：在计算温度和气压。
struct prom_s {
uint16_t factory_setup;
uint16_t c1_pressure_sens;
uint16_t c2_pressure_offset;
uint16_t c3_temp_coeff_pres_sens;
uint16_t c4_temp_coeff_pres_offset;
uint16_t c5_reference_temp;
uint16_t c6_temp_coeff_temp;
uint16_t serial_and_crc;
};
reset
开始转换convert（measure）
计算collect
cycle
{
开始转换convert（measure）
计算collect
}
init的时候就会创建设备文件，供应用层通讯。
/dev/ms5611_spi_int
###########################################################################################################
###########################################################################################################
ORB通讯结构：
这里是定义一个结构体orb_metadata，名字和大小。用于某个ORB通讯内容。
#define ORB_DEFINE(_name, _struct) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct) \
}; struct hack
如下定义：其中input_rc.h是自动生成的。
#include "topics/input_rc.h"
ORB_DEFINE(input_rc, struct input_rc_s);
声明这个结构体：
# define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT
如下声明：
ORB_DECLARE(input_rc);
使用，发送端：
if (_to_input_rc == nullptr) {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);//打开文件只写，没有创建，文件在/obj/
} else {
orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);//发送数据
}
接收端：
RCInput.cpp (ardupilot\libraries\ap_hal_vrbrain): _rc_sub = orb_subscribe(ORB_ID(input_rc));//打开只读。没有创建
int orb_check(int handle, bool *updated)//检查接收是否有数据。
RCInput.cpp (ardupilot\libraries\ap_hal_vrbrain): orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);//接收数据。
###########################################################################################################
###########################################################################################################
滤波：
1.均值，采样次数越多越好，但是不能无限采样，所以
可以升级：采集500(
越大越稳定，但延时)次时，进行一次1/2处理，count=250继续加。效果还是不错的。
2.加权平均
3.低通滤波:一个简单的公式：out += (in - out) * a;其中out是个全局，滤波后的数据，in为输入值，a为0-1系数，越小滤波效果稳定，但是延时严重，0全0；越大越无滤波效果,1跟随输入。0.001稳定但不活跃，0.1以上，滤波效果并不佳。
校准：
校准开始后，先均值滤波，在校准，这样数据才准。
Gauss–Newton algorithm高斯牛顿算法,不断的调节y=f(x,b)中的b，使得采样数据归到这个公式上面，使得惨差和最小
首先公式要确定，如果不确定就不好做了。
理论基础是：泰勒公式（记不起了，理论共识都记不住，利用推导的结果就可以了）
参考：（注：他们的方法残值改为均方差最好。算法的均方差最小时，就不用再尝试了。）
http://blog.csdn.net/tclxspy/article/details/51281811
https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm
高斯牛顿算法：第一有个公式计算y=f(x,b)假设是y=b1*x+b2.第二，有n组数据sample[n][2]
参数b的队列b[2]={b1,b2}我们就是要计算最佳的b1,b2使得公式的惨差r=y-f(x,b)和最小。
根据高斯牛顿算法 b(k+1) = b(k) + (JT * J)^(-1) * JT * r(k-1)
J就是在k点，公式对b求偏导数的集合。
如上：J[n][2] = {{x1 , 0}, {x2, 0}..., {xn, 0}}.n行，2列
JT[2][n]是J的倒置，2行，n列
^(-1)是矩阵的逆。。
其实就是J的计算方式会不一样，其他的都是一样的。
###########################################################################################################
###########################################################################################################
方差variance-均方差-协方差convariance
参考：http://wenku.baidu.com/link?url=UjKdms2IXnxzfiEwmd2PLgZygQ4YHPVh0e5OOrnaqxstDF-pOiDS1PWNmnZtbVOnBlE1M8zkGR8VTgqmg7LdRPYkfhDg4y9mTznVLr60rv_
数学期望，也是平均值的意思。p为概率：E(X) = X1*p(X1） + X2*p(X2） + …… + Xn*p(Xn)；
特殊情况：对于p为1/n，EX=(X1+...+Xn)/n了。
方差：
定义：D(X) = Var(X) = E{[X-E(X)]^2} = E(X^2) - [E(X)]^2 = [X1-E(X)]^2*p1 + [X2-E(X)]^2*p2 +...+ [Xn-E(X)]^2*pn
标准差：
定义：δ(X) = (D(X)) ^ (1/2)。就是开方的意思。
协方差就是用来衡量两个样本之间的相关性有多少，也就是一个样本的值的偏离程度，会对另外一个样本的值偏离产生多大的影响。
协方差：当X=Y时，那就是特殊情况，就等于 -----方差----- 了
Cov(X,Y) = E{[X-E(X)][Y-E(Y)]} = E(XY)-E(X)E(Y)
相关系数：
ρ(xy) = Cov(X,Y) / {[D(X)D(Y)] ^ (1/2)}
###########################################################################################################
###########################################################################################################
卡尔曼滤波器：
参考：https://en.wikipedia.org/wiki/Kalman_filter
首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式 (2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的 covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (4)
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。
卡尔曼的第四个式子：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))；如果H=1，kg为一个常量(假设在0-1),z为测量值，x为滤波后的值。有： x(k) = x(k-1) + kg * (z - x(k-1))和我上面写的低通滤波器是一样的。现在Kg在变化，跟有效的调整输出值。
我做了个实验，如果Q,R不变的话，p一段时间后会变为固定值，也就成了低通滤波器。所以要跟新Q，R
问题：协方差是针对两个变量相关性的计算。但是Q(过程噪声)R(测量噪声)协方差是谁与谁的关系呢？
###########################################################################################################
###########################################################################################################
蜂鸣器控制:
_tempo, 用来计算响铃周期的,一个周期分为响铃段,和不响铃段.
_note_length,计算响铃周期的.
一个周期 = 240.0s/_tempo/_note_length
_silence_length, 一个周期中,不响铃段,剩下的时间时是响铃段.先响铃再沉默.
_note_mode,响铃控制模式, MODE_NORMAL:1/8周期沉默(默认),MODE_STACCATO:1/4周期沉默,其他:0周期沉默
_repeat,是否重复响铃
_octave, 音阶,范围[0-6],有12个音阶,每个音阶有12个音符,音符{A-G}对应{9, 11, 0, 2, 4, 5, 7}.可以计算出84个音符.计算方式:note_to_divisor(0-84)计算出输出频率123-14917.占空比固定为1.
(_note_tab[c - 'A'] + (_octave * 12) + 1) 通过音阶与周期内的音符:计算出音符,范围0-84
(880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f))*2 通过音符计算出输出频率,123-14917
字符串命令:
L + 数字:设置 _note_length,范围[1,+)
T + 数字:设置 _tempo,范围[32,255]
O + 音阶:设置 _octave,范围[0-6]
<: _octave+=1
<: _octave-=1
M + 字节:1.配置 _note_mode, N=MODE_NORMAL,L=MODE_LEGATO,S=MODE_STACCATO; 2. 配置 _repeat, F=false, B=true
P + 时间:停止,经过配置时间后,继续解析.
N + 音符:直接输入音符,范围(0,84]
(A~G) + 音符加减 + 数字(可选): 输入一个音阶里面的音符. 2. 音符加减: (#和+)计算的音符+1, (-)计算的音符-1, 3.数字指定 _note_length