平台:VS2013+matlab2016a
在这里以一个具体的例子来做示范,matlab程序为卡尔曼滤波相关程序,并且把matlab的各个子程序都做成了function函数的形式。卡尔曼滤波相关程序有四个子程序,调用test.m中的test函数可在matlab中显示出结果,输入为一个1x8的向量,输出为一个12x1的向量。程序如下:
function y=test(z)
% clear all;clc;
t=0.5;
RelatObjCoor = [3.75 3.75 0;1.25 3.75 0; 1.25 1.25 0; 3.75 1.25 0];
%RelatObjCoor = [1.25 1.25 0;-1.25 1.25 0; -1.25 -1.25 0; 1.25 -1.25 0];
Init_X = [0;0;-1.75;0;21.2235;0;0;0;1/6*pi;0;0;0];
f=@(x)[x(1)+t*x(2);x(2);x(3)+t*x(4);x(4);x(5)+t*x(6);x(6);x(7)+t*x(8);x(8);x(9)+t*x(10);x(10);x(11)+t*x(12);x(12);];
h=@(x)[856.66*(x(1)+cos(x(7))*cos(x(9))*RelatObjCoor(1,1)+(cos(x(7))*sin(x(9))*sin(x(11))+sin(x(7))*cos(x(11)))*RelatObjCoor(1,2)+(-cos(x(7))*sin(x(9))*cos(x(11))+sin(x(7))*sin(x(11)))*RelatObjCoor(1,3))/(x(5)+sin(x(9))*RelatObjCoor(1,1)-cos(x(9))*sin(x(11))*RelatObjCoor(1,2)+cos(x(9))*cos(x(11))*RelatObjCoor(1,3))+233.05;
860.36*(x(3)-sin(x(7))*cos(x(9))*RelatObjCoor(1,1)+(-sin(x(7))*sin(x(9))*sin(x(11))+cos(x(7))*cos(x(11)))*RelatObjCoor(1,2)+(sin(x(7))*sin(x(9))*cos(x(11))+cos(x(7))*sin(x(11)))*RelatObjCoor(1,3))/(x(5)+sin(x(9))*RelatObjCoor(1,1)-cos(x(9))*sin(x(11))*RelatObjCoor(1,2)+cos(x(9))*cos(x(11))*RelatObjCoor(1,3))+169.4;
856.66*(x(1)+cos(x(7))*cos(x(9))*RelatObjCoor(2,1)+(cos(x(7))*sin(x(9))*sin(x(11))+sin(x(7))*cos(x(11)))*RelatObjCoor(2,2)+(-cos(x(7))*sin(x(9))*cos(x(11))+sin(x(7))*sin(x(11)))*RelatObjCoor(2,3))/(x(5)+sin(x(9))*RelatObjCoor(2,1)-cos(x(9))*sin(x(11))*RelatObjCoor(2,2)+cos(x(9))*cos(x(11))*RelatObjCoor(2,3))&