1.卡尔曼滤波理论
使用或设计卡尔曼滤波时，尤其要谨记以下点：
(1)不管是KF还是EKF，每个时刻的转移矩阵、过程噪声、控制矩阵、控制输入、测量矩阵、测量噪声都可以不是恒定的且通常都不是恒定的，对KF而言它们通常是前后时刻的时间差的函数，对EKF而言它们通常不仅是前后时刻的时间差的函数，还是前一时刻的状态量的函数。
(2)设计状态方程时，尽量不要使用复杂的模型，只要前后时刻足够短，则复杂的模型可以简化。如变速运动，在极短的两个时刻可近似为匀速运动。
(3)测量矩阵设计成多个，每种传感器对应一个测量矩阵，这样的好处是当有相应的测量时就使用相应的测量矩阵，没有相应的测量或完全不可靠的测量则省去基于此测量的更新。
(4)不管是KF还是EKF，直接求状态方程或测量方程对状态量的一阶偏导就是转移矩阵或测量矩阵。
以下我们以运动学为例来分析EKF在运动学的中运用，为简单化，只估计位置，不估计方向。
2.运动学中的状态方程
在极短的两个时刻，我们通常可以假设目标在XYZ三个方向均为匀速运动，于是有状态方程：
(1)位移与速度的关系：s(t+1)=s(t)+v(t)dt #这里不添加控制输入，而将控制输入反映到速度上。
(2)前后时刻速度的关系：v(t+1)=v(t)+h(dt) #这里h(dt)是控制输入(大多时刻为0)，用于处理特殊时刻，如目标突然停止。
被估计量是(sx, sy, sz, vx, vy, vz)
显然，状态方程是线性的(判断是否线性见后)，属于KF工作范围，分别提取XYZ三个方向的状态方程的各状态量的系数可得状态转移矩阵为：
[1, 0, 0, dt, 0, 0,
0, 1, 0, 0, dt, 0,
0,0, 1, 0, 0, dt,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1]
也可以将以上方程看作非线性方程，扩展到EKF工作范围，根据EKF计算状态转移矩阵的方法，分别在XYZ三个方向上求s(t+1)和v(t+1)对(s(t),v(t))的偏导即可得到相同的状态转移矩阵。
判断状态方程是否为线性：首先，确认估计量是哪些，估计量以外的量或这些量的函数都应看作常量，如上面的dt或h(dt)；其次，确认状态量的因变量和自变量，当前时刻t+1的状态量是因变量，前一时刻t的状态量为自变量；最后，确认是否有自变量的非线性函数，如平方、开方、正弦、余弦等。如以上去掉被估计量中的速度，则状态转移矩阵为：
[1, 0, 0
0, 1, 0
0,0, 1
0, 0, 0
0, 0, 0
0, 0, 0]
显然，此时状态转移矩阵应裁剪为3*3的矩阵。同时，v(t)dt应看作控制输入。尽管极短的两个时刻可看作匀速运动，但不同时间的这两个时刻其速度还是有差别的，所以v(t)不可能是恒值，所以加入速度估计还是很有必要的，可以更好地做预测(因为有速度就可以估计两个时刻间的位移)。
我们再来分析一下匀加速运动的情况，此时有状态方程：
(1)位移与速度的关系：s(t+1)=s(t)+v(t)△t+0.5*a(t)*△t*△t #这里不添加控制输入，而将控制输入反映到速度上。
(2)速度与加速度的关系：v(t+1)=v(t)+a(t)△t #这里不添加控制输入，而将控制输入反映到加速度上。
(3)前后时刻加速度的关系：a(t+1)=a(t)+h(t) #这里h(dt)是控制输入(大多时刻为0)，用于处理特殊时刻，如目标突然停止。
被估计量是(sx, sy, sz, vx, vy, vz, ax, ay, az)
分别提取XYZ三个方向的状态方程的状态量的系数或分别在XYZ三个方向上求s(t+1)、v(t+1)、a(t+1)对(s(t),v(t),a(t))的偏导可得状态转移矩阵为：
[1, 0, 0, △t, 0, 0, 0.5△t△t, 0, 0,
0, 1, 0, 0, △t, 0, 0, 0.5△t△t, 0,
0,0, 1, 0, 0, △t, 0, 0, 0.5△t△t,
0, 0, 0, 1, 0, 0, △t, 0, 0,
0, 0, 0, 0, 1, 0, 0, △t, 0,
0, 0, 0, 0, 0, 1, 0, 0, △t,
0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1]
3.运动学中的测量方程
4.卡尔曼滤波计算优化
(1)预测的优化
状态方程中某些估计量的转移矩阵可能是单位矩阵、某些估计量的控制矩阵可能是零矩阵。为此，没必要把所有估计量的转移矩阵或控制矩阵设计为一个大矩阵，增加没必要的计算。对于转移矩阵是单位矩阵且控制矩阵是零矩阵的估计量，直接将前一时刻的值赋给当前时刻即可。对于控制矩阵是零矩阵的估计量，则直接略去控制量的运算操作。即针对估计量的转移矩阵和控制矩阵的实际情况，将估计量分成多组，对每组单独处理，这与Ceres中将参数分成多块和残差分成多块是一样的原理。
(2)更新的优化
相对状态量而言，测量量更有必要分成多组，因为不是所有传感器都能同时工作或者说都能同时正常工作，在每次迭代时只需更新来源于正常工作的传感器的测量，或者至少工作可靠性不那么低的传感器的测量，对于工作可靠性过低的传感器的测量直接不用，从而不仅可以省去基于此测量的更新的计算成本，还可以省去专门针对此次测量去特殊处理相应的协方差(因为可靠性低所以应放大协方差)。
5.使用样例
以下提供一个基于以上运动学的使用样例，封闭在类EKFSim中，说明如下：
(1)关于目标颜色：真实目标为白球、预测目标为蓝球、测量目标为绿球、更新目标为红球。
(2)关于物理单位：为尽量接近真实世界(最初使用随机参数得不到期望的结果)，长度单位默认米，时间单位默认秒，传感器帧率和目标速度等都尽量接近真实世界的情况。
(2)关于自动控制：可设置目标自动运动，运动轨迹是椭圆，长短轴分别约20和80米。
(3)关于手动控制：可设置目标基于鼠标的轨迹运动，目标物理位置值是鼠标像素位置值除以10后的值，之所以除以10是因为除以10以后的尺度与自动控制模式下的场地尺寸相近。
(4)关于维度切换：可设置目标运动于二维世界或三维世界，三维世界时的深度轨迹坐标是两个二维轨迹的坐标的平方和后再开方，二维世界便于直观的展示卡尔曼滤波的效果，三维世界更具有应用扩展性。
(5)关于自动控制的模拟速度：提供参数可设置自动模拟时的快慢，慢时用于查看每帧详细，快时用于快速生成较长的轨迹。
以下是详细代码，依赖于C++14、OpenCV4.x和Spdlog，封装在类EKFSim：estimate+TestMe
1 #include <opencv2/opencv.hpp> 2 #include <opencv2/viz.hpp> 3 #include <spdlog/spdlog.h> 4 using namespace std; 5 using namespace cv; 6 7 class EKFSim 8 { 9 public: 10 static void TestMe(int argc, char** argv) 11 { 12 EKFSim ekfSim; 13 ekfSim.sim2D = true; 14 ekfSim.autoSim = true; 15 ekfSim.timeShowRemains = 100; 16 ekfSim.estimate(); 17 } 18 19 public://Follow physical conditions: meter for distance, second for time 20 bool sim2D = true; 21 bool autoSim = true; 22 double autoVel = 30; 23 double tarSide = 1; 24 double proNoise = 1.0 * tarSide; 25 double meaNoise = 4.0 * tarSide; 26 double worldSide = 100;//ground area is 100m2 27 double dtFrames = 0.05;//that is 20fps 28 int timeShowFirst30 = 30; 29 int timeShowRemains = 1000; 30 void estimate() 31 { 32 //1.CreateWidgets 33 viz::WCoordinateSystem worldCSys(worldSide); 34 viz::WPlane worldGround(Point3d(worldSide / 2, worldSide / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), Size2d(worldSide, worldSide)); 35 viz::WSphere truSphere(Point3d(0, 0, 0), tarSide, 100, viz::Color::white()); 36 viz::WSphere priSphere(Point3d(0, 0, 0), tarSide, 100, viz::Color::blue()); 37 viz::WSphere meaSphere(Point3d(0, 0, 0), tarSide, 100, viz::Color::green()); 38 viz::WSphere postSphere(Point3d(0, 0, 0), tarSide, 100, viz::Color::red()); 39 worldCSys.setRenderingProperty(viz::OPACITY, 0.1); 40 worldGround.setRenderingProperty(viz::OPACITY, 0.1); 41 truSphere.setRenderingProperty(viz::OPACITY, 0.5); 42 priSphere.setRenderingProperty(viz::OPACITY, 0.5); 43 meaSphere.setRenderingProperty(viz::OPACITY, 0.5); 44 postSphere.setRenderingProperty(viz::OPACITY, 0.5); 45 viz::WText stateInfo("Not Started", Point(10, 40), 10); 46 47 //2.ShowWidgets 48 static viz::Viz3d viz3d(__FUNCTION__); 49 viz3d.showWidget("worldCSys", worldCSys); 50 viz3d.showWidget("worldGround", worldGround); 51 viz3d.showWidget("truSphere", truSphere); 52 viz3d.showWidget("priSphere", priSphere); 53 viz3d.showWidget("meaSphere", meaSphere); 54 viz3d.showWidget("postSphere", postSphere); 55 viz3d.showWidget("stateInfo", stateInfo); 56 57 //3.GetMouseXY 58 static bool exitKF = false; 59 static Vec6d truXYZ000; 60 viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void { if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return; exitKF = keyboarEvent.code == ' '; }, this); 61 viz3d.registerMouseCallback([](const viz::MouseEvent& mouseEvent, void* pVizMouse) 62 { 63 double x = mouseEvent.pointer.x / 10; 64 double y = mouseEvent.pointer.y / 10; 65 double z = ((EKFSim*)pVizMouse)->sim2D ? 0 : 0.8 * std::sqrt(x * x + y * y); 66 truXYZ000 = Vec6d(x, y, z, 0, 0, 0); 67 }, this); 68 69 //4.KalmanFilter 70 Mat_<Vec6d> truDatas; 71 Mat_<Vec6d> priDatas; 72 Mat_<Vec6d> meaDatas; 73 Mat_<Vec6d> postDatas; 74 cv::KalmanFilter kalman(6, 3, 3, CV_64F);//Sensors: Camera3D 75 for (int64 k = 0; ; ++k)//The constant can be put outside the loop 76 { 77 //0.GenerateGT 78 double dt = dtFrames; 79 Vec6d truData = truXYZ000; 80 if (autoSim) 81 { 82 static double theta = 0; 83 theta += (autoVel * dt) / (worldSide * 0.5); 84 truData[0] = worldSide / 2 + worldSide * 0.8 * std::cos(theta); 85 truData[1] = worldSide / 2 + worldSide * 0.2 * std::sin(theta); 86 truData[2] = sim2D ? 0 : std::sqrt(truData[0] * truData[0] + truData[1] * truData[1]); 87 } 88 if (truDatas.rows > 0) for (int k = 0; k < 3; ++k) truData[k + 3] = (truData[k] - truDatas(truDatas.rows - 1)[k]) / dt; 89 90 //1.TransitionMatrix 91 kalman.transitionMatrix = Mat_<double>({ kalman.transitionMatrix.rows, kalman.transitionMatrix.cols }, { 92 1, 0, 0, dt, 0, 0, 93 0, 1, 0, 0, dt, 0, 94 0, 0, 1, 0, 0, dt, 95 0, 0, 0, 1, 0, 0, 96 0, 0, 0, 0, 1, 0, 97 0, 0, 0, 0, 0, 1 }); 98 99 //2.ProcessNoiseMatrix 100 cv::setIdentity(kalman.processNoiseCov, proNoise); 101 102 //3.MeasurementNoiseMatrix 103 cv::setIdentity(kalman.measurementNoiseCov, meaNoise); 104 Vec6d meaData = truData + Vec6d(truData.randn(0, meaNoise).val); 105 if (sim2D) meaData[2] = meaData[5] = 0; 106 107 //4.ControlAction: ControlMatrix and ControlInput 108 kalman.controlMatrix = Mat_<double>::eye(kalman.transitionMatrix.rows, 32); 109 Mat_<double> ctlIn(kalman.controlMatrix.cols, 1, 0.);//Control value can be exerted on control input or control matrix or both, depending on the actual task 110 Mat_<double> priOut = kalman.predict(ctlIn);//Here just show how to use control part and not no use actually because of zero control input 111 Vec6d priData(priOut.ptr<double>()); 112 113 //5.MeasurementAction: MeasurementMatrix and MeasurementInput 114 kalman.measurementMatrix = Mat_<double>::eye(kalman.measurementMatrix.size());; 115 Mat_<double> meaIn(kalman.measurementMatrix.rows, 1, meaData.val); 116 Mat_<double> postOut = kalman.correct(meaIn); 117 Vec6d postData(postOut.ptr<double>()); 118 119 //6.ShowResults 120 auto cvarr2str = [](InputArray v)->string 121 { 122 Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT); 123 string dst; fmtd->reset(); 124 for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str); 125 return dst; 126 }; 127 stateInfo.setText(fmt::format("Press space to exit and show trajectories TruDataLast: {} dt: {} TruData(White): {} {} PriData(Blue): {} {} MeaData(Green): {} {} PostData(Red): {} {}", 128 cvarr2str(truDatas.rows > 1 ? truDatas(truDatas.rows - 1).t() : truData.t()), dt, 129 cvarr2str(truData.t()), truDatas.rows, 130 cvarr2str(priData.t()), priDatas.rows, 131 cvarr2str(meaData.t()), meaDatas.rows, 132 cvarr2str(postData.t()), postDatas.rows)); 133 truDatas.push_back(truData); 134 priDatas.push_back(priData); 135 meaDatas.push_back(meaData); 136 postDatas.push_back(postData); 137 truSphere.setPose(cv::Affine3d(Matx33d::eye(), Vec3d(truData[0], truData[1], truData[2]))); 138 priSphere.setPose(cv::Affine3d(Matx33d::eye(), Vec3d(priData[0], priData[1], priData[2]))); 139 meaSphere.setPose(cv::Affine3d(Matx33d::eye(), Vec3d(meaData[0], meaData[1], meaData[2]))); 140 postSphere.setPose(cv::Affine3d(Matx33d::eye(), Vec3d(postData[0], postData[1], postData[2]))); 141 viz3d.spinOnce(k < 30 ? timeShowFirst30 : timeShowRemains, false); 142 if (exitKF) break; 143 } 144 145 //5.ShowTrajectories 146 Mat_<cv::Affine3d> truTrajs; for (int k = 0; k < truDatas.rows; ++k) truTrajs.push_back(cv::Affine3d(Matx33d::eye(), Vec3d(truDatas(k).val))); 147 Mat_<cv::Affine3d> priTrajs; for (int k = 0; k < priDatas.rows; ++k) priTrajs.push_back(cv::Affine3d(Matx33d::eye(), Vec3d(priDatas(k).val))); 148 Mat_<cv::Affine3d> meaTrajs; for (int k = 0; k < meaDatas.rows; ++k) meaTrajs.push_back(cv::Affine3d(Matx33d::eye(), Vec3d(meaDatas(k).val))); 149 Mat_<cv::Affine3d> postTrajs; for (int k = 0; k < postDatas.rows; ++k) postTrajs.push_back(cv::Affine3d(Matx33d::eye(), Vec3d(postDatas(k).val))); 150 viz3d.showWidget("truTrajs", viz::WTrajectorySpheres(truTrajs, 100, 0.2, viz::Color::white(), viz::Color::white())); 151 viz3d.showWidget("priTrajs", viz::WTrajectorySpheres(priTrajs, 100, 0.2, viz::Color::blue(), viz::Color::blue())); 152 viz3d.showWidget("meaTrajs", viz::WTrajectorySpheres(meaTrajs, 100, 0.2, viz::Color::green(), viz::Color::green())); 153 viz3d.showWidget("postTrajs", viz::WTrajectorySpheres(postTrajs, 100, 0.2, viz::Color::red(), viz::Color::red())); 154 viz3d.spin(); 155 } 156 }; 157 158 int main(int argc, char** argv) { EKFSim::TestMe(argc, argv); return 0; }
