最近简单看了一下kalman滤波，这里先写些笔记
卡尔曼滤波说的是“两套系统“得到的计算量或者测量量，一个是根据系统状态预测出来的下一个状态的各种参量，另一个是实际测量值，那么我们怎么在这两套系统中找到更合适的值。
说白了，它所谓的这两套系统都是服从高斯分布的，他们的乘也符合高斯分布，根据这两个高斯分布的方差的大小，调整到合适的方差选择
F有的地方有人喜欢写成A。B是控制矩阵, x应该加的是均值为零的噪声，所以就省略掉没写，B有的文献喜欢写成C（control），P，协方差矩阵，有的地方也喜欢写成V的
H是观测矩阵，是你最后的观测值和待分析状态之间的矩阵转移关系,有地地方喜欢写成C，比如 我只能观测到所有状态的和，那么，测量值（z） = [1 1 1 1 1...] * [ x1 x2 x2....]T ，R 就是1*1维度的，另外 y 就是 测量余量（误差），注意，y 和 x 没什么自变量应变量之间的关系，别看到x，y就想到 y =f(x)
x 是某些状态值（不一定能观察到，或者是某些变量的中间传递函数），有点隐含状态的意思
kalman描述的是一些原始轨迹或者规律不方便描述，但有状态之间的转移是了解的（A矩阵），另外 H 是一个转换矩阵
它能 H * x（隐含状态） = 观测值
p0 和Q 都可以先设为对角矩阵
Q 和 R的设定，记住，Q/(Q+R) 就是卡尔曼增益的收敛值
Q越大，说明测量值可靠，R越大，说明预测值越可靠
X的初始值和方差可以简单设置，后面会收敛，难的是Q和R的值
扩展Extended kalman filter
g函数是状态转移函数了，不需要A矩阵了，参与运算的Gt在计算方差的时候出现
h函数是观测函数了，Ht是H函数的求偏导
记忆公式我也不擅长，我找了两个工具箱，分别是kalman 和扩展kalman 的代码
做了一些改动，上传至：
https://download.csdn.net/download/book_bbyuan/11729042
我reference的是一个很好的 KF和 EKF的例子
http://www.pyrunner.com/weblog/2018/04/12/kalman-example/
我按它给出的python的函数，自己适当改动了一下，使用matlab 改写，下图是我做的实验
原问题描述的是做8字运行的小车的轨迹，下三图分别是 x y位置，角速度，速度的真实状态和含噪数据
首先使用kalman滤波器，我用的是
https://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html
中剥离出来，它实际就三个函数是核心的，kalman_filter.m kalman_update.m kalman_filter.m, 我直接杂糅到一个函数里面了
状态：四个: x dx/dt dx2/dt2 y dy/dt dy2/dt2 ,
，x y是位置
观测值：两个， x_gps y_gps
得到结论如下: 黑色是实际运行轨迹，蓝色点是观测值，橙红色是KF滤波之后结果
下面尝试使用扩展卡尔曼滤波器
这时候状态值还是 上面说的四元组
但观测值，这次是四个：
使用的扩展卡尔曼滤波器，reference这里：
https://www.cbcity.de/das-extended-kalman-filter-einfach-erklaert
https://www.mathworks.com/matlabcentral/fileexchange/18189-learning-the-extended-kalman-filter
原公式需要求各个状态值的偏导数
jacobi行列式的原来定义
https://www.codetd.com/article/2131570 有一个例子
他的特点是不用记公式，只要描述x中的不同状态变量之间的关系就可以
求偏导的事情交给工具就行了
黑色是实际运行轨迹，蓝色点是观测值，橙红色是KF滤波之后结果
扩展的效果要优于一般的kalman滤波器
代码上传到：