云台设计

云台设计构想及编程应用（C语言—–C＋＋转换未完成）

控制平台设计构想
传感器读取初始数据
卡尔曼滤波算法获取yaw，roll，pitch的角度
将数据转化为电机的目标值
测量值如果和目标值相等
读取磁编码测量其位置值
测量值与目标值不相等
经过位置环，速度环，电流环
改变电机位置

状态预测：
𝑋(𝑘,𝑘−1)=𝐴(𝑘,𝑘−1)𝑋(𝑘−1)
或者
𝑋(𝑘,𝑘−1)=𝐴(𝑘,𝑘−1)𝑋(𝑘−1)+B(k)u(k)

这里X(k,k-1)是利用k-1时刻预测的当前状态结果
X(k-1)是k-1时刻最优值
A(k,k-1)是X(k-1)状态下的变换矩阵，它是算法对状态变量进行预测的依据
B(k)是作用在控制量上的变换矩阵，在大多数实际情况下并没有控制收益
u(k)是当前状态的控制增益，一般没有这个变量，可以设为0

协方差阵预测：
P(𝑘,𝑘−1)=𝐴(𝑘,𝑘−1)𝑃(𝑘−1) 𝐴^𝑇 (𝑘,𝑘−1)+𝐵(𝑘,𝑘−1)𝑄(𝑘−1)𝐵^𝑇 (𝑘,𝑘−1)

P(𝑘,𝑘−1)是k时刻系统协方差矩阵
P(k-1)是k-1时刻系统协方差矩阵
Q(k-1)是系统过程的协方差矩阵

滤波增益：
根据k时刻的协方差矩阵的预测值计算卡尔曼收益
𝐾(𝑘)=(𝑝(𝑘,𝑘−1) 𝐶^𝑇 (𝑘))/((𝐶(𝑘)𝑃(𝑘,𝑘−1) 𝐶^𝑇 (𝑘)+𝑅(𝑘))
k(k)是卡尔曼增益，是滤波的中间结果
C(k)是对象的预测矩阵
R(k)是对象测量噪声的协方差矩阵，是一个数值，作为已知条件输入滤波器。
这个值过大过小都会使滤波效果变差，且R(k)取值越小收敛越快,所以可以通过实验手段寻找合适的R(k)值再利用它进行真实的滤波.

协方差阵估计：
目的：为了求k时刻的协方差矩阵（为得到k+1时刻的卡尔曼输出值做准备）
𝑃(𝑘)=(1−𝐾(𝑘)𝐶(𝑘))𝑃(𝑘,𝑘−1)
当p(k,k-1)为一个一维矩阵的时候,C(k)是1

状态估计：
𝑋(k)=𝑋(𝑘,𝑘−1）+𝐾(𝑘)(𝑌(𝑘)−𝐶(𝑘)𝑋(𝑘,𝑘−1))
X(k)是k时刻状态变量的最优估计值
Y(K)是对象的测量

//1. 结构体类型定义
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP；//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={0.02,0,0,0,0.001,0.543};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
 float kalmanFilter(KFP *kfp,float input)
 {
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P / (kfp->NOw_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out；
 }

/**
 *调用卡尔曼滤波器 实践
 */
int height;
int kalman_height=0;
kalman_height = kalmanFilter(&KFP_height,(float)height);