学习ardupilot一点心得

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(测量噪声)协方差是谁与谁的关系呢？

###########################################################################################################
###########################################################################################################