基于Kalman filter 的北斗卫星三维坐标后数据处理
基于Kalman filter 的北斗卫星三维坐标后数据处理
Kalman filter 基本介绍
- 1. 应用场景？
模型内事件状态转换是线性的，噪声干扰是高斯的
公式表示：
P（Xt|Xt-1）=N(AX+B,Q) Xt=AXt-1+B+W
P(Yt|Xt)=N(HX+C,R) Yt=HX+C+V W ,V meet Normal distribution( 0,sigma)
制定H ，降维 我们只需要某一个维度状态
P(Xt|Y1,Y2,….Yt)
- 2. Kalman filter求什么？
求（Xt|Y1*Y2*Y3….Yt）的性质。
Xt是某事物t时刻真实的状态。Yt是t时刻观察事物状态的观测值。
我们已知前t个的时刻的观测值， 计算t时刻 隐状态的某些 参数。
比如概率P，均值E(Xt_bar)，方差Var（Xt_bar）。
3.基本思想：
没有真值， 只有传感器的观测值，具有对目标系统建立数学模型获得预期值。
我们要用预期值和观测值来逼近真值。
举个例子：
蓝色的模型预期值，红色的传感器的值，我们通过把两个正态分布乘起来，计算概率密度函数的均值，来寻找最佳估计。
为什么是乘（1*1=1）？ 为什么能乘（∫fdx=1）？
4.算法步骤
4.1列方程模型
匀速直线运动+干扰， （静止则速度为0），模型中的加速度和速度不是恒定的，是参数迭代计算出来的。
初值是我们定义的。 但是初值作用不是很大，经过少量迭代即可趋于稳定。
P是FX的协方差矩阵，
Qk是状态转移协方差矩阵。联系两个连续离散时刻的协方差矩阵（实际上是噪声Δw的协方差矩阵，后面说明），Qk参数可以以自己调节。
F是两个连续时刻状态的过渡矩阵。，wk是扰动。
4.2 寻找迭代 闭环
如何保证能算法收敛？
u为输入，y输出模型预期值。Y_hat是 真实传感器的值。
收敛的条件：调整KC使得A-KC小于0，指数函数就会随着时间收敛。
我把K取值为负数。结果误差是7*10^304 mm
我们写出P（xt|y1：t-1），P（xt|y1：t）的值。
为什么要写这个P（xt|y1：t-1）P（xt|y1：t）？HMM，当转移矩阵，测量矩阵，初始概率确定后，模型就确定的。我们写出P（xt|y1：t-1），P（xt|y1：t）是量化。
公式（2），冒号代表从1到t-1，
p(xt|y1:t−1)代表预测，已知t-1时刻的观测值，求xt时刻的状态。
当有了p(yt|xt)中的yt 后，就可以对p(xt|y1:t)中的 Xt实现更新。
注意prediction 中 p(xt|y1:t) 与 p(xt|y1:t−1)已经 实现递归
p(xt|y1:t−1)满足高斯分布，p(yt|xt)满足高斯分布，高斯乘以高斯结果还是高斯分布。所以p(xt|y1:t)也是高斯的。
说明算法处理同一组数据，结果只能限定在一个范围内，绝对没有一组数据是结果一样的。
实际上：t=1时刻有 P（X1|Y1）~N(μ_hat_1，sigma_hat_1) update
t=2时刻有 P（X2|Y1）~N(μ_bar_2，sigma_bar_2) prediction
P（X2|Y1，Y2）~N(μ_bar_2，sigma_bar_2) update
同样的：t时刻
P(Xt|Y1,Y2,,,Yt)~ N(μ_hat_t，sigma_hat_t) update
P(Xt|Y1:Yt)~N(μ_bar_t，sigma_bar_t)) prediction
我们要做什么？把参数设置好了时候，每次不断求update 和prediction的值，
也就是如何从上一次高斯的两个参数mean和var，得到下一次高斯的两个参数mean和var。
分别对随机变量X和线性变换后的随机变量AX计算mean和var。
Q噪声的Δw的协方差矩阵，是Δw，不是w的协方差，所以不为0。
Q我在计算时取为定值。（实际上不应该是定值）噪声一定会变，可是我没办法预估。
通过观测定值效果也很不错。我已经把结果震荡限制在1mm内。
有了P（xt|y1：t-1），P（xt|y1：t）后，就可以求t时刻状态值的mean和var。
第一种计算方法：
另一种计算的方法：
思路相同：利用模型预期值和测量值，来寻找最佳估计值
已知状态的Xt，观测的Yt，边缘分布。
如何求Xt，Yt的边缘分布呢？--结果显然是高斯的。查表套公式即可。
求出XY期望的mean和var ，
同样的还是会得到这三个公式。
实际上是mean和var两个公式。
（注意K是被推导的）只不过要查表应用随机变量乘积的高斯分布获取乘积的pdf。
4.3迭代更新。
4.4 观测滤波结果，不符合预期则调整参数
- 中值滤波数据预处理，
- 计算预处理后的标准差 ，作为加入噪声的参考值，使用计算预处理后的均值，作为代替真值的参考值（大样本均值思想）
- 设置模型初始条件，设置参数，模型确定了，P,H参数是固定的。F中第一行第二个元素是时间，我对照表格取一分钟，F的其他参数也是固定的。联系两个FX的协方差的矩阵Q的取值很大程度影响误差，R是调整预期值与传感器值权重的参数，相信预期值多，则把R调小些，反之亦反。
- 开始迭代，输入卡拉曼滤波公式，开始接待计算误差。
- 目前参数我已经调整到 把结果稳定在了1mm内
- 部分结果展示，经过少量迭代，结果趋于稳定。
横轴是位置，纵轴是速度，很明显，速度是非常接近0.
- 同时也观察不同Q值对误差影响，Q值建议调成大于1
计划：进行实时数据检验算法准确性，效果不佳，尝试非线性非高斯粒子滤波。
考虑到天气等原因影响，尝试使用参数学习，找出最佳参数，实现参数自适应。
欢迎大家随时打断我，感谢大家提出自己的见解!
