二维匀速 Kalman：完整协方差传播

题目

状态为位置和速度 $\mathbf x=[x,v]^T$，初值 $[0,0]^T$，$P=I_2$。$$F=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix},\quad Q=q\begin{bmatrix}\Delta t^4/4&\Delta t^3/2\\\Delta t^3/2&\Delta t^2\end{bmatrix},\quad H=\begin{bmatrix}1&0\end{bmatrix},\quad R=r.$$ 每步先预测再位置观测更新；用 Joseph 形式更新协方差。 本题假设每个采样区间内加速度为独立随机常量 $a_k$，均值为零，方差 $q=\operatorname{Var}(a_k)$，单位为 $\mathrm{m^2/s^4}$；因此 $Q=qGG^T$，$G=[\Delta t^2/2,\Delta t]^T$。观测噪声参数 $r$ 是位置测量方差，单位为 $\mathrm{m^2}$。这不是连续白加速度噪声谱密度模型：若采用谱密度 $q_c$（单位 $\mathrm{m^2/s^3}$），应使用另一离散化 $Q_c=q_c\begin{bmatrix}\Delta t^3/3&\Delta t^2/2\\\Delta t^2/2&\Delta t\end{bmatrix}$，不可混用两种参数。

回到相关教材 →

输入

$N,\Delta t,q,r$，然后 $N$ 个位置观测。$\Delta t>0$，$q\ge0$，$r>0$。 位置采用 m，时间采用 s；$q$ 是每区间常加速度方差而非连续噪声谱密度，$r$ 是位置观测方差。

输出

每步 $x,v,P_{00},P_{01},P_{11}$，共 $5N$ 个实数。

示例

输入

12 1 0 0.3
0 0 0 0 0 0 0 0 0 0 0 0

期望输出

0.0 0.0 0.2608695652173913 0.13043478260869565 0.5652173913043478 0.0 0.0 0.23510971786833856 0.15047021943573666 0.2163009404388715 0.0 0.0 0.21447721179624665 0.10455764075067024 0.08847184986595173 0.0 0.0 0.18917134367778143 0.07131066358534169 0.04258831297457906 0.0 0.0 0.1665442796299016 0.050668233220737255 0.023351446614774556 0.0 0.0 0.14777554214173932 0.03755868544600938 0.01408450704225352 0.0 0.0 0.1323951871435114 0.028852158707689736 0.009117781756109748 0.0 0.0 0.11971778128049239 0.02281768370487127 0.0062298281167905145 0.0 0.0 0.10915341641598625 0.018478727975934677 0.004440624552356398 0.0 0.0 0.10024480971608432 0.015260865418243093 0.0032747273709979995 0.0 0.0 0.09264644481720494 0.012811403540899071 0.002483170840688844 0.0 0.0 0.08609748836626295 0.010905159581968884 0.001927204926123813

// q: variance of independent, constant acceleration per sample interval [m^2/s^4].
// r: position measurement variance [m^2]; NOT continuous acceleration PSD.
#include <iostream>
#include <iomanip>
#include <cmath>
#include <algorithm>
#include <vector>
#include <array>
#include <queue>
#include <tuple>
#include <limits>
using namespace std;

// Reference implementation: optional; solve the starter before opening.
int main() {
    cout << setprecision(17);
    int n;
    double dt,q,r;
    cin>>n>>dt>>q>>r;
    double x=0,v=0,p00=1,p01=0,p11=1;
    while(n--) {
        double z;
        cin>>z;
        x+=dt*v;
        double a=p00+2*dt*p01+dt*dt*p11+q*pow(dt,4)/4,b=p01+dt*p11+q*pow(dt,3)/2,c=p11+q*dt*dt,k0=a/(a+r),k1=b/(a+r),e=z-x;
        x+=k0*e;
        v+=k1*e;
        p00=(1-k0)*(1-k0)*a+k0*k0*r;
        p01=(1-k0)*(b-k1*a)+k0*k1*r;
        p11=c-2*k1*b+k1*k1*a+k1*k1*r;
        cout<<x<<" "<<v<<" "<<p00<<" "<<p01<<" "<<p11<<"\n";
    }
}